Go to the documentation of this file.00001 #include <ros/ros.h>
00002 #include <sensor_msgs/Range.h>
00003 #include <mrp2_hardware/mrp2_serial.h>
00004
00005 ros::Publisher sonar1_pub;
00006 ros::Publisher sonar2_pub;
00007 ros::Publisher sonar3_pub;
00008 ros::Publisher sonar4_pub;
00009 ros::Publisher sonar5_pub;
00010 ros::Publisher sonar6_pub;
00011 ros::Publisher sonar7_pub;
00012
00013 MRP2_Serial *sonar_serial;
00014
00015 int use_sonar, sonar_baud;
00016 std::string sonar_port;
00017
00018
00019 int main(int argc, char **argv)
00020 {
00021 ros::init(argc, argv, "mrp2_sonar");
00022 ros::NodeHandle nh_;
00023
00024 ros::param::param<std::string>("~sonar_port", sonar_port, "/dev/mrp2_ftdi_MRP2SNR001");
00025 ros::param::param<int32_t>("~sonar_baud", sonar_baud, 115200);
00026
00027 sonar1_pub = nh_.advertise<sensor_msgs::Range>("sonar_1", 100);
00028 sonar2_pub = nh_.advertise<sensor_msgs::Range>("sonar_2", 100);
00029 sonar3_pub = nh_.advertise<sensor_msgs::Range>("sonar_3", 100);
00030 sonar4_pub = nh_.advertise<sensor_msgs::Range>("sonar_4", 100);
00031 sonar5_pub = nh_.advertise<sensor_msgs::Range>("sonar_5", 100);
00032 sonar6_pub = nh_.advertise<sensor_msgs::Range>("sonar_6", 100);
00033 sonar7_pub = nh_.advertise<sensor_msgs::Range>("sonar_7", 100);
00034
00035 sonar_serial = new MRP2_Serial(sonar_port, sonar_baud, "8N1", false);
00036 sonar_serial->set_read_timeout(0.05);
00037
00038 ros::Rate loop_rate(20);
00039
00040 while(ros::ok())
00041 {
00042 std::vector<int> sonar_vals;
00043
00044 sonar_vals.reserve(20);
00045 sonar_vals.clear();
00046
00047 sonar_vals = sonar_serial->get_sonars(true);
00048
00049 sensor_msgs::Range sonar;
00050
00051 sonar.radiation_type = sensor_msgs::Range::ULTRASOUND;
00052 sonar.header.stamp = ros::Time::now();
00053 sonar.field_of_view = 0.6108652;
00054 sonar.max_range = 0.4;
00055 sonar.min_range = 0.02;
00056
00057 sonar.header.frame_id = "base_sonar_1_link";
00058 sonar.range = ((double)sonar_vals[0] / 100);
00059 sonar1_pub.publish(sonar);
00060
00061 sonar.header.frame_id = "base_sonar_2_link";
00062 sonar.range = ((double)sonar_vals[1] / 100);
00063 sonar2_pub.publish(sonar);
00064
00065 sonar.header.frame_id = "base_sonar_3_link";
00066 sonar.range = ((double)sonar_vals[2] / 100);
00067 sonar3_pub.publish(sonar);
00068
00069 sonar.header.frame_id = "base_sonar_4_link";
00070 sonar.range = ((double)sonar_vals[3] / 100);
00071 sonar4_pub.publish(sonar);
00072
00073 sonar.header.frame_id = "base_sonar_5_link";
00074 sonar.range = ((double)sonar_vals[4] / 100);
00075 sonar5_pub.publish(sonar);
00076
00077 sonar.header.frame_id = "base_sonar_6_link";
00078 sonar.range = ((double)sonar_vals[5] / 100);
00079 sonar6_pub.publish(sonar);
00080
00081 sonar.header.frame_id = "base_sonar_7_link";
00082 sonar.range = ((double)sonar_vals[6] / 100);
00083 sonar7_pub.publish(sonar);
00084
00085 ros::spinOnce();
00086 loop_rate.sleep();
00087 }
00088 }