Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include <stdlib.h>
00038 #include <stdio.h>
00039 #include <math.h>
00040 #include <string.h>
00041
00042 #include "ardusim/Ardusim.h"
00043
00044 using namespace ardusim;
00045
00046 int main(int argc, char** argv)
00047 {
00048 ros::init(argc, argv, "roomba_sonar_node");
00049
00050 ros::NodeHandle n;
00051 ros::NodeHandle pn("~");
00052
00053 std::vector<lse_sensor_msgs::Range> range_msgs;
00054
00055 ros::Publisher range_pub = n.advertise<lse_sensor_msgs::Range>("/sonars", 1);
00056
00057
00058 std::string port;
00059 pn.param<std::string>("port", port, "/dev/ttyUSB1");
00060 std::string frame_id;
00061 pn.param<std::string>("frame_id", frame_id, "/base_sonar");
00062
00063 Ardusim ardusim(port);
00064
00065 int requests[] = {ARDUSIM_RANGE};
00066 ardusim.setRequests(requests, 1);
00067
00068 ros::Rate r(10);
00069 while(ros::ok())
00070 {
00071 if(ardusim.getSensorData(100))
00072 {
00073 if(ardusim.getRange(&range_msgs))
00074 {
00075 range_msgs[0].header.frame_id = frame_id;
00076 range_msgs[0].header.frame_id += "_0";
00077 range_msgs[1].header.frame_id = frame_id;
00078 range_msgs[1].header.frame_id += "_1";
00079 range_msgs[2].header.frame_id = frame_id;
00080 range_msgs[2].header.frame_id += "_2";
00081 range_msgs[3].header.frame_id = frame_id;
00082 range_msgs[3].header.frame_id += "_3";
00083 range_msgs[4].header.frame_id = frame_id;
00084 range_msgs[4].header.frame_id += "_4";
00085
00086 for(int i=0 ; i<range_msgs.size() ; i++)
00087 {
00088 range_msgs[i].field_of_view = 0.523598776;
00089 range_msgs[i].min_range = 0.03;
00090 range_msgs[i].max_range = 2.00;
00091 range_pub.publish(range_msgs[i]);
00092 }
00093 }
00094 }
00095
00096 r.sleep();
00097 }
00098
00099 return(0);
00100 }
00101
00102
00103