laser_crossing_detector_wrapper.cpp
Go to the documentation of this file.
00001 /* Wrapper for LaserCrossingDetector
00002  *
00003  * Parameter passing occurs through strings. The serialization/deserialization
00004  * in Python is done in crossing_detector/crossing_detector.py, class CrossingDetector.
00005  */
00006 #include <algorithm>
00007 #include <string>
00008 #include <vector>
00009 #include <sstream>
00010 
00011 #include <boost/python.hpp>
00012 #include <python2.7/Python.h>
00013 
00014 #include <ros/serialization.h>
00015 
00016 #include <sensor_msgs/LaserScan.h>
00017 #include <lama_msgs/Crossing.h>
00018 #include <lama_msgs/Frontier.h>
00019 
00020 #include <crossing_detector/laser_crossing_detector.h>
00021 #include <crossing_detector/wrapper_utils.h>
00022 
00023 namespace bp = boost::python;
00024 namespace rs = ros::serialization;
00025 
00026 using sensor_msgs::LaserScan;
00027 using crossing_detector::LaserCrossingDetector;
00028 using lama_msgs::Crossing;
00029 using lama_msgs::Frontier;
00030     
00031 /* Wrapper class for LaserCrossingDetector
00032  *
00033  * Functions arguments and returned types are strings and list of strings with
00034  * serialized content.
00035  */
00036 class LaserCrossingDetectorWrapper : public LaserCrossingDetector
00037 {
00038   public:
00039 
00040     LaserCrossingDetectorWrapper(const double frontier_width, const double max_frontier_angle=0.785) :
00041       LaserCrossingDetector(frontier_width, max_frontier_angle)
00042     {
00043     }
00044 
00045     std::string crossingDescriptor(const std::string str_scan, const bool normalize=false)
00046     {
00047 
00048       LaserScan scan = from_python<LaserScan>(str_scan);
00049       Crossing crossing = LaserCrossingDetector::crossingDescriptor(scan, normalize);
00050 
00051       return to_python(crossing);
00052     }
00053 
00054     std::vector<std::string> frontiers(const std::string str_scan, const bool normalize=false)
00055     {
00056       LaserScan scan = from_python<LaserScan>(str_scan);
00057       std::vector<Frontier> frontiers = LaserCrossingDetector::frontiers(scan, normalize);
00058       std::vector<std::string> str_frontiers;
00059       str_frontiers.reserve(frontiers.size());
00060       for (size_t i = 0; i < frontiers.size(); ++i)
00061       {
00062         str_frontiers.push_back(to_python(frontiers[i]));
00063       }
00064       return str_frontiers;
00065     }
00066 };
00067 
00068 BOOST_PYTHON_MODULE(laser_crossing_detector_wrapper_cpp)
00069 {
00070   bp::class_<LaserCrossingDetectorWrapper>("LaserCrossingDetectorWrapper", bp::init<double, double>())
00071     .def("crossingDescriptor", &LaserCrossingDetectorWrapper::crossingDescriptor)
00072     .def("frontiers", &LaserCrossingDetectorWrapper::frontiers)
00073     ;
00074 
00075   bp::to_python_converter<std::vector<std::string, std::allocator<std::string> >, vector_to_python<std::string> >();
00076 }


crossing_detector
Author(s): Gaël Ecorchard , Karel Košnar , Vojtěch Vonásek
autogenerated on Thu Jun 6 2019 22:02:06