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 #include <sick_tim/sick_tim_datagram_test.h>
00036
00037 #include <sick_tim/sick_tim310s01_parser.h>
00038
00039 namespace sick_tim
00040 {
00041
00042 SickTimDatagramTest::SickTimDatagramTest(AbstractParser* parser) :
00043 parser_(parser)
00044 {
00045
00046 dynamic_reconfigure::Server<sick_tim::SickTimConfig>::CallbackType f;
00047 f = boost::bind(&sick_tim::SickTimDatagramTest::update_config, this, _1, _2);
00048 dynamic_reconfigure_server_.setCallback(f);
00049
00050 pub_ = nh_.advertise<sensor_msgs::LaserScan>("scan_from_datagram", 1000);
00051 sub_ = nh_.subscribe("datagram", 1, &SickTimDatagramTest::datagramCB, this);
00052 }
00053
00054 SickTimDatagramTest::~SickTimDatagramTest()
00055 {
00056 delete parser_;
00057 }
00058
00059 void SickTimDatagramTest::datagramCB(const std_msgs::String::ConstPtr &datagram_msg)
00060 {
00061 sensor_msgs::LaserScan scan_msg;
00062
00063 std::vector<char> str(datagram_msg->data.begin(), datagram_msg->data.end());
00064 str.push_back('\0');
00065
00066 int success = parser_->parse_datagram(&str[0], datagram_msg->data.length(), config_, scan_msg);
00067 if (success == ExitSuccess)
00068 pub_.publish(scan_msg);
00069 else
00070 ROS_ERROR("parse_datagram returned %d!", success);
00071 }
00072
00073 void SickTimDatagramTest::check_angle_range(SickTimConfig &conf)
00074 {
00075 if (conf.min_ang > conf.max_ang)
00076 {
00077 ROS_WARN("Minimum angle must be greater than maximum angle. Adjusting min_ang.");
00078 conf.min_ang = conf.max_ang;
00079 }
00080 }
00081
00082 void SickTimDatagramTest::update_config(sick_tim::SickTimConfig &new_config, uint32_t level)
00083 {
00084 check_angle_range(new_config);
00085 config_ = new_config;
00086 }
00087
00088 }
00089
00090 int main(int argc, char **argv)
00091 {
00092 ros::init(argc, argv, "sick_tim_datagram_test");
00093
00094 sick_tim::SickTim310S01Parser* parser = new sick_tim::SickTim310S01Parser();
00095 sick_tim::SickTimDatagramTest s(parser);
00096
00097 ros::spin();
00098
00099 return 0;
00100 }