robot mapper please
Hi, I have ultrasound sensor connecter to arduino and i want to create a map. I try to convert range sonar to laser scan, but i don't recieve data of sensor. what can i do to receive data on rvis and create a map on it please? This is my code and a picture of rvis:
#include "math.h"
#if defined(ARDUINO) && ARDUINO >= 100
#include "Arduino.h"
#else
#include "WProgram.h"
#endif
// ROS includes
#include <ros.h>
#include <ros/time.h>
#include <sensor_msgs/LaserScan.h>
sensor_msgs::LaserScan CDLaser_msg;
#define echoPin A1
#define trigPin A0
//
float f_angle_min;
float f_angle_max;
float f_angle_increment;
float f_time_increment;
float f_scan_time;
float f_range_min;
float f_range_max;
float f_ranges[5]; // max of 30 measurements
float f_intensities[5];
int publisher_timer;
ros:: Publisher pub_Laser("LaserData", &CDLaser_msg);
ros::NodeHandle nh;
long duration;
float getRange(){
int val = 0;
for(int i=0; i<4; i++){
digitalWrite(trigPin, LOW);
delayMicroseconds(2);
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);
duration = pulseIn(echoPin, HIGH);
val += duration;
}
val=(val/232.8);
return val;
}
void setup()
{
nh.initNode();
nh.advertise(pub_Laser);
pinMode(trigPin, OUTPUT);
pinMode(echoPin, INPUT);
f_angle_min = -1.57;
f_angle_max = 1.57;
f_angle_increment = 0.785; // 3.14/4 - 5 measurement points
f_time_increment = 10;
f_scan_time = 4;
f_range_min = 0.1;
f_range_max = 30;
CDLaser_msg.ranges_length = 5;
CDLaser_msg.intensities_length = 5;
// create the test data
for (int z = 0 ; z<5; z++)
{
f_ranges[z] = getRange();
f_intensities[z] = 10;
}
}
void loop()
{
if (millis() > publisher_timer)
{
CDLaser_msg.header.stamp = nh.now();
CDLaser_msg.header.frame_id = "laser_frame";
CDLaser_msg.angle_min = f_angle_min;
CDLaser_msg.angle_max = f_angle_max;
CDLaser_msg.angle_increment = f_angle_increment;
CDLaser_msg.time_increment = f_time_increment;
CDLaser_msg.scan_time = f_scan_time;
CDLaser_msg.range_min = f_range_min;
CDLaser_msg.range_max = f_range_max;
for (int z = 0 ; z<5; z++)
{
CDLaser_msg.ranges[z] = f_ranges[z];
CDLaser_msg.intensities[z] = f_intensities[z];
}
publisher_timer = millis() + 1000;
pub_Laser.publish(&CDLaser_msg);
}
nh.spinOnce();
delay(50);
}