neurali:ros_husky_laser_versione_20150804
ricevo la propriocezione (accelerometro)
#include <ros/ros.h> #include <geometry_msgs/Twist.h> #include <sensor_msgs/LaserScan.h> #include <geometry_msgs/Vector3.h> #include <sensor_msgs/Imu.h> #include <iostream> #include <algorithm> /*min_element*/ #include <cmath> /* round*/ class TeleopTurtle { public: TeleopTurtle(); private: void laserCallback(const sensor_msgs::LaserScan::ConstPtr& laser); void velCallback(const sensor_msgs::Imu::ConstPtr& vel); ros::NodeHandle nh_; //int linear_, angular_; //double l_scale_, a_scale_; ros::Publisher vel_pub_; ros::Subscriber laser_sub_; ros::Subscriber vel_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); vel_sub_ = nh_.subscribe<sensor_msgs::Imu>("/imu/data", 10, &TeleopTurtle::velCallback, this); } void TeleopTurtle::velCallback(const sensor_msgs::Imu::ConstPtr& vel) { // http://docs.ros.org/jade/api/sensor_msgs/html/msg/Imu.html // geometry_msgs::Vector3 vel_lin = vel->linear; float acc_lin = vel->linear_acceleration.x; // non e' la velocita'.... float vel_ang = vel->angular_velocity.z; // sarebbero float 64 std::cout << round(acc_lin*10) << "," << round(10*vel_ang) << std::endl; // arrotondare... /* non esiste un sensore di velocita ma solo un accelerometro che ti segnala una frenata improvvisa per misurare la velocita, solo due soluzioni: montare un gps o misurare la distanza con gli oggetti nel tempo, ma se non ci sono oggetti..... */ } void TeleopTurtle::laserCallback(const sensor_msgs::LaserScan::ConstPtr& laser) { unsigned int quantita = laser->ranges.size(); unsigned int copia[9]; //le distanze si misurano in dm, da adesso in poi. 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... } 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_20150804.txt · Last modified: 2020/06/08 22:20 by 127.0.0.1