Go to the documentation of this file.00001 #include <ros/ros.h>
00002 #include <sensor_msgs/Range.h>
00003 #include <std_msgs/Int32.h>
00004 #include <string>
00005 #include <utility>
00006 #include <limits>
00007
00008
00009 #define VREP_LOWER_BOUND 1
00010 #define VREP_UPPER_BOUND 10
00011 #define VREP_END VREP_UPPER_BOUND - VREP_LOWER_BOUND + 1
00012
00013 #define VREP_ANGLE_DETECTION 0.5236 // in [rad]
00014 #define VREP_MIN_DIST_DETECTION 0.0
00015 #define VREP_MAX_DIST_DETECTION 0.0
00016
00017
00018 std::string tf_prefix;
00019
00028 void bumperSensorData_cb(const ros::Publisher& publisher, std::size_t sensor_n, const std_msgs::Int32::ConstPtr& msg)
00029 {
00030 sensor_msgs::Range range_msg;
00031
00032
00033 range_msg.radiation_type = 3;
00034 range_msg.field_of_view = VREP_ANGLE_DETECTION;
00035 range_msg.min_range = VREP_MIN_DIST_DETECTION;
00036 range_msg.max_range = VREP_MAX_DIST_DETECTION;
00037
00038 constexpr auto inf = std::numeric_limits<decltype(range_msg.range)>::infinity();
00039 if(msg->data)
00040 range_msg.range = -inf;
00041 else
00042 range_msg.range = inf;
00043
00044
00045 range_msg.header.stamp = ros::Time::now();
00046 range_msg.header.frame_id = tf_prefix + "/bumperSensors/sensor" + std::to_string(sensor_n);
00047
00048 publisher.publish(std::move(range_msg));
00049 }
00050
00051 int main(int argc, char **argv)
00052 {
00053 ros::init(argc, argv, "bumperSensor");
00054 ros::NodeHandle n("~");
00055
00056
00057
00058 int vrep_no = 0;
00059
00060 std::string tf_prefix_path;
00061 if (n.searchParam("tf_prefix", tf_prefix_path))
00062 {
00063 n.getParam(tf_prefix_path, tf_prefix);
00064 }
00065
00066 std::string vrep_index_path;
00067 if (n.searchParam("vrep_index", vrep_index_path))
00068 {
00069 n.getParam(vrep_index_path, vrep_no);
00070 }
00071
00072
00073 std::vector<ros::Publisher> hal_publishers;
00074 hal_publishers.reserve(VREP_END);
00075 for (int i = 0; i < VREP_END; ++i)
00076 {
00077 hal_publishers.push_back(
00078 n.advertise<sensor_msgs::Range>(
00079 "sensor" + std::to_string(i) + "/Range",
00080 1000));
00081 }
00082
00083
00084 std::vector<ros::Subscriber> vrep_subscribers;
00085 vrep_subscribers.reserve(VREP_END);
00086
00087 auto it = hal_publishers.cbegin();
00088 std::size_t vrep_i = VREP_LOWER_BOUND;
00089 for (; it != hal_publishers.cend(); ++vrep_i, ++it)
00090 {
00091
00092 boost::function<void(const std_msgs::Int32::ConstPtr&)> callback =
00093 std::bind(bumperSensorData_cb, *it, std::distance(hal_publishers.cbegin(), it), std::placeholders::_1);
00094
00095 vrep_subscribers.push_back(
00096 n.subscribe(
00097 "/vrep/i" + std::to_string(vrep_no) + "_Pioneer_p3dx_bumper" + std::to_string(vrep_i),
00098 1000,
00099 std::move(callback)
00100 ));
00101 }
00102
00103 ROS_INFO("HAL(VREP): BumperSensor node initialized");
00104
00105
00106 ros::spin();
00107
00108 return 0;
00109 }