tibi_dabo_pitch_estimator_sin_laser_vertical_alg_node.cpp

Go to the documentation of this file.

#include "tibi_dabo_pitch_estimator_sin_laser_vertical_alg_node.h"
#include <stdio.h>
#include <string.h>
#include <cmath>
TibiDaboPitchEstimatorSinLaserVerticalAlgNode::TibiDaboPitchEstimatorSinLaserVerticalAlgNode(void) :
  algorithm_base::IriBaseAlgorithm<TibiDaboPitchEstimatorSinLaserVerticalAlgorithm>(),
a1_(0),
b1_(0),
front_security_distance_(0.7),
rear_security_distance_(1.2)
{
  //init class attributes if necessary
  //this->loop_rate_ = 2;//in [Hz]

  // [init publishers]
  this->MarkerArray_lines_publisher_ = this->public_node_handle_.advertise<visualization_msgs::MarkerArray>("MarkerArray_lines", 100);
  this->front_laser_filtrado_publisher_ = this->public_node_handle_.advertise<sensor_msgs::LaserScan>("front_laser_filtrado", 100);
this->rear_laser_filtrado_publisher_ = this->public_node_handle_.advertise<sensor_msgs::LaserScan>("rear_laser_filtrado", 100);
  
  // [init subscribers]
  this->front_laser_subscriber_ = this->public_node_handle_.subscribe("front_laser", 100, &TibiDaboPitchEstimatorSinLaserVerticalAlgNode::front_laser_callback, this);
   this->rear_laser_subscriber_ = this->public_node_handle_.subscribe("rear_laser", 100, &TibiDaboPitchEstimatorSinLaserVerticalAlgNode::rear_laser_callback, this);
  this->pitch_subscriber_ = this->public_node_handle_.subscribe("pitch", 100, &TibiDaboPitchEstimatorSinLaserVerticalAlgNode::pitch_callback, this);
  
  // [init services]
  
  // [init clients]
  
  // [init action servers]
  
  // [init action clients]
}

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

void TibiDaboPitchEstimatorSinLaserVerticalAlgNode::mainNodeThread(void)
{
  // [fill msg structures]
  //this->MarkerArray_msg_.data = my_var;
  //this->LaserScan_msg_.data = my_var;
  
  // [fill srv structure and make request to the server]
  
  // [fill action structure and make request to the action server]




  // [publish messages]

  this->MarkerArray_lines_publisher_.publish(this->MarkerArray_msg_);
 //en teoria esta aquí no va...
 // this->front_laser_filtrado_publisher_.publish(this->LaserScan_front_msg_);
}

/*  [subscriber callbacks] */
void TibiDaboPitchEstimatorSinLaserVerticalAlgNode::front_laser_callback(const sensor_msgs::LaserScan::ConstPtr& msg) 
{ 
 // ROS_INFO("TibiDaboPitchEstimatorSinLaserVerticalAlgNode::front_laser_callback: New Message Received"); 


this->front_laser_mutex_.enter(); 

  //this->laser_mutex_.enter();                 
   this->alg_.lock(); 
                
        double  distance_intersection_=60,distance_point_laser_=0;
        
        LaserScan_front_msg_=*msg; //coge todo el mensage del front laser y lo copia a mi laser filtrado. (que saldra este por salida o este, con posicion del punto a 0.0, para flitrarlo.
        
        for( unsigned int i =0; i < msg->ranges.size()-1  ; ++i )
        {
        
                double inicial_point_x = 0.28;
                double inicial_point_y =0.44;
                                        
                double intersection_y = (0.44-b1_)/a1_+0.28 ;   
                double intersection_x = b1_+0.44 ;
                
                double angle = msg->angle_max;
                
                if(a1_>0){
                
                        distance_intersection_=sqrt((inicial_point_x-intersection_x)*(inicial_point_x-intersection_x)+(inicial_point_y-intersection_y)*(inicial_point_y-intersection_y));
                        distance_point_laser_=msg->ranges[i]; 
                        
                }       
                                 
                if(distance_intersection_< distance_point_laser_+front_security_distance_){
                
                        LaserScan_front_msg_.ranges[i]=0.0;
                
                }
                
        }

this->front_laser_filtrado_publisher_.publish(this->LaserScan_front_msg_);
        
        //función para mostrar por pantalla los laseres del front laser para usarlos con matlab.
        

                double angle = msg->angle_min;

                for( unsigned int i =0 ; i < msg->ranges.size()-1  ; ++i )
                {

                        front_laser_point_x=msg->ranges[i] * cos(angle)-0.28;
                        front_laser_point_y=0.44-msg->ranges[i] * sin(angle);
                        marker_point_front_laser_node(front_laser_point_x,front_laser_point_y);
                        
                        //printf("%lf %lf\n", msg->ranges[i] * cos(angle)-0.28,0.44-msg->ranges[i] * sin(angle));
                        angle += msg->angle_increment;

                }
        //printf("\n");
        //printf("\n");
        //printf("\n");
        
          this->alg_.unlock();
  //unlock previously blocked shared variables 
  //this->laser_mutex_.exit(); 

 this->front_laser_mutex_.exit(); 
}

void TibiDaboPitchEstimatorSinLaserVerticalAlgNode::rear_laser_callback(const sensor_msgs::LaserScan::ConstPtr& msg) 
{ 

  //ROS_INFO("TibiDaboPitchEstimatorSinLaserVerticalAlgNode::rear_laser_callback: New Message Received"); 


this->rear_laser_mutex_.enter(); 


   this->alg_.lock(); 
                
        double  distance_intersection_=60,distance_point_laser_=0;

        LaserScan_rear_msg_=*msg;
        
        for( unsigned int i =0; i < msg->ranges.size()-1  ; ++i )
        {

                double inicial_point_x = 0.28;
                double inicial_point_y =0.44;
                                                
                double angle = msg->angle_max;
                
                if(a1_<0){
                
                double intersection_y = (0.44-b1_)/(-a1_)+0.28 ;        
                double intersection_x = b1_+0.44 ;
                
                        distance_intersection_=sqrt((inicial_point_x-intersection_x)*(inicial_point_x-intersection_x)+(inicial_point_y-intersection_y)*(inicial_point_y-intersection_y));
                        distance_point_laser_=msg->ranges[i]; 
                        
                }
                                 
                if(distance_intersection_< distance_point_laser_+rear_security_distance_){

                        LaserScan_rear_msg_.ranges[i]=0.0;
        
                }
                
        }       
        
this->rear_laser_filtrado_publisher_.publish(this->LaserScan_rear_msg_);
        
        //función para mostrar por pantalla los laseres del rear laser para usarlos con matlab.
        
        
                        double angle = msg->angle_min;

                for( unsigned int i =0 ; i < msg->ranges.size()-1  ; ++i )
                {
                
                        front_laser_point_x=msg->ranges[i] * cos(angle)-0.28;
                        front_laser_point_y=0.44-msg->ranges[i] * sin(angle);
                        marker_point_front_laser_node(front_laser_point_x,front_laser_point_y);
                        //printf("%lf %lf\n", msg->ranges[i] * cos(angle)-0.28,0.44-msg->ranges[i] * sin(angle));
                        angle += msg->angle_increment;

                }
        
        //printf("\n");
        //printf("\n");
        //printf("\n");
        
        this->alg_.unlock();
  //unlock previously blocked shared variables 

  this->rear_laser_mutex_.exit(); 
}

void TibiDaboPitchEstimatorSinLaserVerticalAlgNode::pitch_callback(const iri_segway_rmp_msgs::SegwayRMP200Status::ConstPtr& msg) 
{ 
 // ROS_INFO("TibiDaboPitchEstimatorSinLaserVerticalAlgNode::pitch_callback: New Message Received"); 


  //use appropiate mutex to shared variables if necessary 
  this->alg_.lock(); 
  this->pitch_mutex_.enter(); 

  pitch=msg->pitch_angle;
  
function_markers(pitch);

  //unlock previously blocked shared variables 
  this->alg_.unlock(); 
  this->pitch_mutex_.exit(); 


}

//function Markers (pitch and line flor, y=a1_x+b1_)

void TibiDaboPitchEstimatorSinLaserVerticalAlgNode::function_markers(double pitch)
{

pitch_arrow_marker_.points.clear();
floor_line_marker_.points.clear();
arrow_intersection_.points.clear();
        //marker pitch  
                geometry_msgs::Point ros_point;
                pitch_arrow_marker_.id=4;               
                pitch_arrow_marker_.header.frame_id = "/dabo/base_link";
                pitch_arrow_marker_.header.stamp = ros::Time();
                pitch_arrow_marker_.ns = "pitch";
                pitch_arrow_marker_.type = visualization_msgs::Marker::ARROW;           
                pitch_arrow_marker_.action = visualization_msgs::Marker::ADD;
                pitch_arrow_marker_.lifetime = ros::Duration(1.0f);
                
        //arrow_marker4_.points.reserve(2);
        if ( pitch_arrow_marker_.points.size() <2)
                {
                        double x;
                        ros_point.x = 0;
                        ros_point.z =0;
                        ros_point.y = 0;
                        pitch_arrow_marker_.points.push_back(ros_point);
                        if(tan(pitch)>0){
                        x=-5.0;
                        ros_point.x = -5/sqrt(5*5+((-1/tan(pitch))*(-5))*((-1/tan(pitch))*(-5)));
                        }
                        else{
                        x=5.0;
                        ros_point.x = 5/sqrt(5*5+((-1/tan(pitch))*5)*((-1/tan(pitch))*5));
                        }
                        ros_point.z= ((-1/tan(pitch))*x)/sqrt(x*x+((-1/tan(pitch))*x)*((-1/tan(pitch))*x));// pitch line. witch Position 90º respect the floor.
                        ros_point.y = 0.0;
                        pitch_arrow_marker_.points.push_back(ros_point);
        
                }
           
                pitch_arrow_marker_.scale.x = 0.05;
                pitch_arrow_marker_.scale.y = 0.05;
                pitch_arrow_marker_.scale.z = 0.05;
        
                pitch_arrow_marker_.color.a = 1.0;
                pitch_arrow_marker_.color.r = 0.7;
                pitch_arrow_marker_.color.g = 0.3;
                pitch_arrow_marker_.color.b = 0.0;
                        
        //marker floor line.
        
        a1_=tan(pitch);
        b1_=0;
                geometry_msgs::Point ros_point2;
                floor_line_marker_.id=0;                
                floor_line_marker_.header.frame_id = "/dabo/base_link";
                floor_line_marker_.header.stamp = ros::Time();
                floor_line_marker_.ns = "line floor";
                floor_line_marker_.type = visualization_msgs::Marker::LINE_STRIP;
                floor_line_marker_.action = visualization_msgs::Marker::ADD;
                floor_line_marker_.lifetime = ros::Duration(1.0f);
                floor_line_marker_.points.reserve(2);
                
        if ( floor_line_marker_.points.size() <2)
                {
                        ros_point2.x = -10;
                        ros_point2.z =a1_*ros_point2.x+b1_;
                        ros_point2.y = 0;
                        floor_line_marker_.points.push_back(ros_point2);        
                        ros_point2.x = 10;
                        ros_point2.z=a1_*ros_point2.x+b1_;
                        ros_point2.y = 0;
                        floor_line_marker_.points.push_back(ros_point2);
                }
           
                floor_line_marker_.scale.x = 0.05;
                floor_line_marker_.color.a = 1.0;
                floor_line_marker_.color.r = 0.0;
                floor_line_marker_.color.g = 1.0;
                floor_line_marker_.color.b = 0.0;       
                        
//marker intersection point     
                geometry_msgs::Point ros_point3;
                arrow_intersection_.id=5;               
                arrow_intersection_.header.frame_id = "/dabo/base_link";
                arrow_intersection_.header.stamp = ros::Time();
                arrow_intersection_.ns = "intersection_point";
                arrow_intersection_.type = visualization_msgs::Marker::ARROW;
                //intersection_marker5_.type = visualization_msgs::Marker::SPHERE;              
                arrow_intersection_.action = visualization_msgs::Marker::ADD;
                arrow_intersection_.lifetime = ros::Duration(1.0f);
        
                /*intersection_marker5_.pose.position.x=(0.44-b1)/a1+0.28;
                intersection_marker5_.pose.position.z=b1+0.44;//intersection point front_laser with floor.
                intersection_marker5_.pose.position.y=0;
                intersection_marker5_.points.push_back(ros_point5);*/
                //ROS_INFO("intersection point X=%lf intersection point Y= %lf" ,(0.44-b1)/a1,b1);      

                if ( arrow_intersection_.points.size() <2)
                {
                        ros_point3.x = 0.28;
                        ros_point3.z =0.44;
                        ros_point3.y = 0; //mirar.
                        arrow_intersection_.points.push_back(ros_point3);
                                                        
                        ros_point3.x = (0.44-b1_)/a1_+0.28;     
                        ros_point3.z=b1_+0.44;// pitch line. witch Position 90º respect the floor.
                        ros_point3.y = 0.0;
                        arrow_intersection_.points.push_back(ros_point3);
        
                }

                arrow_intersection_.scale.x = 0.1;
                arrow_intersection_.scale.y = 0.1;
                arrow_intersection_.scale.z = 0.1;
        
                arrow_intersection_.color.a = 1.0;
                arrow_intersection_.color.r = 0.5;
                arrow_intersection_.color.g = 0.0;
                arrow_intersection_.color.b = 0.5;              
                        
                        
MarkerArray_msg_.markers.push_back(pitch_arrow_marker_);        
MarkerArray_msg_.markers.push_back(floor_line_marker_);
MarkerArray_msg_.markers.push_back(arrow_intersection_);

}


void TibiDaboPitchEstimatorSinLaserVerticalAlgNode::marker_point_front_laser_node(double front_laser_point_x, double front_laser_point_y)
{

        //marker point  front laser             
                distance_front_laser_marker6_.points.clear();
                geometry_msgs::Point ros_point6;
                distance_front_laser_marker6_.id=6;             
                distance_front_laser_marker6_.header.frame_id = "/dabo/base_link";
                distance_front_laser_marker6_.header.stamp = ros::Time();
                distance_front_laser_marker6_.ns = "puntos_front_laser";
                //distance_front_laser_marker6_.type = visualization_msgs::Marker::ARROW;
                distance_front_laser_marker6_.type = visualization_msgs::Marker::SPHERE;        
                distance_front_laser_marker6_.action = visualization_msgs::Marker::ADD;
                distance_front_laser_marker6_.lifetime = ros::Duration(1.0f);   

                /*if ( distance_front_laser_marker6_.points.size() <2)
                {
                        ros_point6.x = 0.28 ;
                        ros_point6.z = 0.44 ;
                        ros_point6.y = 0 ; //mirar.
                        distance_front_laser_marker6_.points.push_back(ros_point6);
                                                        
                        ros_point6.x = distance_point_laser;    
                        ros_point6.z= 0 ;// pitch line. witch Position 90º respect the floor.
                        ros_point6.y = 0.0 ;
                        distance_front_laser_marker6_.points.push_back(ros_point6);
        
                }*/

                distance_front_laser_marker6_.pose.position.x=front_laser_point_x;
                distance_front_laser_marker6_.pose.position.y=front_laser_point_y;
                distance_front_laser_marker6_.pose.position.z=0.44;

                distance_front_laser_marker6_.scale.x = 0.2;
                distance_front_laser_marker6_.scale.y = 0.2;
                distance_front_laser_marker6_.scale.z = 0.2;
        
                distance_front_laser_marker6_.color.a = 1.0;
                distance_front_laser_marker6_.color.r = 1.0;
                distance_front_laser_marker6_.color.g = 0.0;
                distance_front_laser_marker6_.color.b = 0.0;
                MarkerArray_msg_.markers.clear();
        MarkerArray_msg_.markers.push_back(distance_front_laser_marker6_);
}       
        

/*  [service callbacks] */

/*  [action callbacks] */

/*  [action requests] */




void TibiDaboPitchEstimatorSinLaserVerticalAlgNode::node_config_update(Config &config, uint32_t level)
{
  this->alg_.lock();

  this->alg_.unlock();
}

void TibiDaboPitchEstimatorSinLaserVerticalAlgNode::addNodeDiagnostics(void)
{
}

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