User Tools

Site Tools


neurali:ros_husky_laser_versione_20150803

Differences

This shows you the differences between two versions of the page.

Link to this comparison view

neurali:ros_husky_laser_versione_20150803 [2018/04/25 07:55]
neurali:ros_husky_laser_versione_20150803 [2020/06/08 22:20] (current)
Line 1: Line 1:
  
 +Trova il minimo di 9 misure
 +
 +<code cpp>
 +#include <ros/ros.h>
 +#include <geometry_msgs/Twist.h>
 +#include <sensor_msgs/LaserScan.h>
 +#include <iostream>
 +#include <algorithm> /*min_element*/
 +#include <cmath> /* round*/
 +
 +class TeleopTurtle
 +{
 +public:
 +  TeleopTurtle();
 + 
 +private:
 +  void laserCallback(const sensor_msgs::LaserScan::ConstPtr& laser);
 + 
 +  ros::NodeHandle nh_;
 + 
 +  //int linear_, angular_;
 +  //double l_scale_, a_scale_;
 +  ros::Publisher vel_pub_;
 +  ros::Subscriber laser_sub_;
 + 
 +};
 + 
 + 
 +TeleopTurtle::TeleopTurtle()
 +{
 + /*
 +  nh_.param("axis_linear", linear_, linear_);
 +  nh_.param("axis_angular", angular_, angular_);
 +  nh_.param("scale_angular", a_scale_, a_scale_);
 +  nh_.param("scale_linear", l_scale_, l_scale_);
 + */
 + 
 +  //vel_pub_ = nh_.advertise<geometry_msgs::Twist>("cmd_vel", 1);
 + 
 +  laser_sub_ = nh_.subscribe<sensor_msgs::LaserScan>("/scan", 10, &TeleopTurtle::laserCallback, this);
 + 
 +}
 +
 +void TeleopTurtle::laserCallback(const sensor_msgs::LaserScan::ConstPtr& laser)
 +
 +/*  //riceve e trasforma da joy a vel
 +  geometry_msgs::Twist vel;
 +  vel.angular = a_scale_*joy->axes[angular_];
 +  vel.linear = l_scale_*joy->axes[linear_];
 +  vel_pub_.publish(vel);*/ 
 +
 +  //std::cout << laser->range_min << "," << laser->range_max << " - "; //0.1 - 6
 +  unsigned int quantita = laser->ranges.size(); // 720 valori, compresi tra -90°,+90°: troppi.
 +  
 +  unsigned int copia[9]; // le distanze si misurano in dm, da adesso in poi. Sono intere e più stabili
 +  
 +  for(unsigned int i=0,j=0; i < quantita; j++,i=i+quantita/8-1) 
 +  /*poiche' quantita e' multiplo di 8 , 360, allora otterro' 9 valori:
 +  v[0],v[44],v[89],v[134],v[179],v[224],v[269],v[314],v[359]*/
 +  {
 +        copia[j]=round(10*(laser->ranges[i])); // cast, altrimenti il valore oscilla pure... 
 +       //std::cout << copia[j] << "|"; //debug
 +  }
 +  
 +    unsigned int* p_min = std::min_element(&(copia[0]),&(copia[8]));
 +    std::cout << "o" << ": "<< round(copia[4]) << std::endl;          //front view
 +    std::cout << (p_min-&(copia[0])) << ": "<< (*p_min) << std::endl; //posiz. minimo
 +}
 +
 +int main(int argc, char** argv)
 +
 +  ros::init(argc, argv, "husky_driver");
 +  TeleopTurtle teleop_turtle;
 +  ros::Rate rate(10);  //aggiunto ma non cambia
 +  ros::spin();
 +}
 +</code>