no way to compare when less than two revisions

Differences

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

@@ Line 1: / Line 1: @@
+ricevo la propriocezione (accelerometro)
+<code cpp>
+#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();
+}
+</code>