neurali:ros_husky_laser_versione_20150803
Trova il minimo di 9 misure
#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(); }
neurali/ros_husky_laser_versione_20150803.txt · Last modified: 2020/06/08 22:20 by 127.0.0.1