Hi
I'm trying to visualize a map using 3 ultrasonic sensor and arduino mega 2560. I developed my own node to generate LaserScan from 3 sonar range. Algorithm using for mapping is [gmapping](http://wiki.ros.org/gmapping).
rang_to_laserscan.cpp :
#include "ros/ros.h"
#include "sensor_msgs/Range.h"
#include "sensor_msgs/LaserScan.h"
float data[3];
void u1Callback(const sensor_msgs::Range::ConstPtr& r1)
{
data[0]=r1->range;
}
void u2Callback(const sensor_msgs::Range::ConstPtr& r2)
{
data[1]=r2->range;
}
void u3Callback(const sensor_msgs::Range::ConstPtr& r3)
{
data[2]=r3->range;
}
int main(int argc, char** argv){
ros::init(argc, argv, "rang_to_laserscan");
ros::NodeHandle n;
ros::Subscriber sub_u1 = n.subscribe("/ultrasound1", 1000, u1Callback);
ros::Subscriber sub_u2 = n.subscribe("/ultrasound2", 1000, u2Callback);
ros::Subscriber sub_u3 = n.subscribe("/ultrasound3", 1000, u3Callback);
ros::Publisher pub1 = n.advertise("/scan1", 50);
int num_readings = 3;
float laser_frequency = 40;
ros::Rate loop_rate(10);
while (ros::ok())
{
ros::Time scan_time = ros::Time::now();
sensor_msgs::LaserScan scan;
scan.header.stamp = scan_time;
scan.header.frame_id = "range_to_laser_frame";
scan.angle_min = -0.785398;
scan.angle_max = 0.785398;
scan.angle_increment = 1.570796 / num_readings;
scan.time_increment = (1 / laser_frequency) / (num_readings);
scan.range_min = 0.02;
scan.range_max = 4.0;
scan.ranges.resize(num_readings);
for(int i=0;i
#include
#include
long duration;float d;int i=0;
int T[]={23,25,27,29,31,33,35,37,39,41};
int E[]={22,24,26,28,30,32,34,36,38,40};
ros::NodeHandle nh;
sensor_msgs::Range range_msg;
ros::Publisher pub_range1("/ultrasound1", &range_msg);
ros::Publisher pub_range2("/ultrasound2", &range_msg);
ros::Publisher pub_range3("/ultrasound3", &range_msg);
/*ros::Publisher pub_range4("/ultrasound4", &range_msg);
ros::Publisher pub_range5("/ultrasound5", &range_msg);
ros::Publisher pub_range6("/ultrasound6", &range_msg);
ros::Publisher pub_range7("/ultrasound7", &range_msg);
ros::Publisher pub_range8("/ultrasound8", &range_msg);
ros::Publisher pub_range9("/ultrasound9", &range_msg);
ros::Publisher pub_range10("/ultrasound10", &range_msg);*/
char frameid[] = "/ultrasound";
float getRange(int T,int E)
{
digitalWrite(T,HIGH);
delayMicroseconds(10);
digitalWrite(T,LOW);
duration=pulseIn(E,HIGH);
d=duration/58;
delayMicroseconds(5);
return d/100;
}
void setup()
{
nh.initNode();
nh.advertise(pub_range1);
nh.advertise(pub_range2);
nh.advertise(pub_range3);
/*nh.advertise(pub_range4);
nh.advertise(pub_range5);
nh.advertise(pub_range6);
nh.advertise(pub_range7);
nh.advertise(pub_range8);
nh.advertise(pub_range9);
nh.advertise(pub_range10);*/
range_msg.radiation_type = sensor_msgs::Range::ULTRASOUND;
range_msg.header.frame_id = frameid;
range_msg.field_of_view = 1.570796;
range_msg.min_range = 0.02;
range_msg.max_range = 4.0;
for(i=0;i<3;i++){
pinMode(T[i], OUTPUT);
digitalWrite(T[i], LOW);
pinMode(E[i], INPUT);
}
}
long range_time;
void loop()
{
if ( millis() >= range_time ){
range_msg.range = getRange(T[0],E[0]);
range_msg.header.stamp = nh.now();
pub_range1.publish(&range_msg);
range_msg.range = getRange(T[1],E[1]);
range_msg.header.stamp = nh.now();
pub_range2.publish(&range_msg);
range_msg.range = getRange(T[2],E[2]);
range_msg.header.stamp = nh.now();
pub_range3.publish(&range_msg);
/*range_msg.range = getRange(T[3],E[3]);
range_msg.header.stamp = nh.now();
pub_range4.publish(&range_msg);
range_msg.range = getRange(T[4],E[4]);
range_msg.header.stamp = nh.now();
pub_range5.publish(&range_msg);
range_msg.range = getRange(T[5],E[5]);
range_msg.header.stamp = nh.now();
pub_range6.publish(&range_msg);
range_msg.range = getRange(T[6],E[6]);
range_msg.header.stamp = nh.now();
pub_range7.publish(&range_msg);
range_msg.range = getRange(T[7],E[7]);
range_msg.header.stamp = nh.now();
pub_range8.publish(&range_msg);
range_msg.range = getRange(T[8],E[8]);
range_msg.header.stamp = nh.now();
pub_range9.publish(&range_msg);
range_msg.range = getRange(T[9],E[9]);
range_msg.header.stamp = nh.now();
pub_range10.publish(&range_msg);*/
range_time = millis() + 50;
}
nh.spinOnce();
}
command :
terminal1$ roscore
terminal2$ rosrun rosserial_python serial_node.py /dev/ttyACM0
terminal3$ rosrun irobot range_to_laserscan
terminal4$ rosrun gmapping slam_gmapping scan:=scan1
terminal5$ rosrun rviz rviz
I am using ros kinetic in ubuntu 16.04
The problem is when I run rviz the warning "No map received" occurs.
thanks
↧