├── .gitignore
├── navigation_layers
    ├── CHANGELOG.rst
    ├── CMakeLists.txt
    └── package.xml
├── range_sensor_layer
    ├── CHANGELOG.rst
    ├── CMakeLists.txt
    ├── README.md
    ├── cfg
    │   └── RangeSensorLayer.cfg
    ├── costmap_plugins.xml
    ├── include
    │   └── range_sensor_layer
    │   │   └── range_sensor_layer.h
    ├── package.xml
    ├── scripts
    │   └── send_message.py
    └── src
    │   └── range_sensor_layer.cpp
└── social_navigation_layers
    ├── CHANGELOG.rst
    ├── CMakeLists.txt
    ├── cfg
        └── ProxemicLayer.cfg
    ├── costmap_plugins.xml
    ├── include
        └── social_navigation_layers
        │   ├── proxemic_layer.h
        │   └── social_layer.h
    ├── package.xml
    └── src
        ├── passing_layer.cpp
        ├── proxemic_layer.cpp
        └── social_layer.cpp


/.gitignore:
--------------------------------------------------------------------------------
1 | build/
2 | 


--------------------------------------------------------------------------------
/navigation_layers/CHANGELOG.rst:
--------------------------------------------------------------------------------
 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 2 | Changelog for package navigation_layers
 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 4 | 
 5 | 0.4.0 (2018-08-31)
 6 | ------------------
 7 | * Clean up with roscompile
 8 | * Contributors: David V. Lu
 9 | 
10 | 0.2.4 (2014-12-10)
11 | ------------------
12 | 
13 | 0.2.3 (2014-12-10)
14 | ------------------
15 | 


--------------------------------------------------------------------------------
/navigation_layers/CMakeLists.txt:
--------------------------------------------------------------------------------
1 | cmake_minimum_required(VERSION 2.8.3)
2 | project(navigation_layers)
3 | find_package(catkin REQUIRED)
4 | catkin_metapackage()
5 | 


--------------------------------------------------------------------------------
/navigation_layers/package.xml:
--------------------------------------------------------------------------------
 1 | <package format="2">
 2 |     <name>navigation_layers</name>
 3 |     <version>0.5.0</version>
 4 |     <description>Extra navigation layers.</description>
 5 |     <maintainer email="davidvlu@gmail.com">David V. Lu!!</maintainer>
 6 |     <author>David V. Lu!!</author>
 7 |     <license>BSD</license>  
 8 | 
 9 |     <buildtool_depend>catkin</buildtool_depend>
10 |     <exec_depend>range_sensor_layer</exec_depend>
11 |     <exec_depend>social_navigation_layers</exec_depend>
12 |     <export>
13 |         <metapackage/>
14 |     </export>
15 | </package>
16 | 


--------------------------------------------------------------------------------
/range_sensor_layer/CHANGELOG.rst:
--------------------------------------------------------------------------------
 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 2 | Changelog for package range_sensor_layer
 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 4 | 
 5 | 0.4.0 (2018-08-31)
 6 | ------------------
 7 | * Clean up with roscompile
 8 | * range sensor layer: Remove parameter max_angle (`#38 <https://github.com/DLu/navigation_layers/issues/38>`_)
 9 |   Any value set for max_angle gets overwritten in updateCostmap() so the
10 |   parameter can safely be removed.
11 | * Issue `#22 <https://github.com/DLu/navigation_layers/issues/22>`_: on updateCostmap, only touch cells within the triangular projection of the sensor cone. I also add a parameter to regulate (and deactivate) this feature (`#30 <https://github.com/DLu/navigation_layers/issues/30>`_)
12 | * Add buffer clearing when calling deactivate() or activate(). (`#33 <https://github.com/DLu/navigation_layers/issues/33>`_)
13 | * Contributors: David V. Lu, Jorge Santos Simón, Krit Chaiso, nxdefiant
14 | 
15 | 0.2.4 (2014-12-10)
16 | ------------------
17 | 
18 | 0.2.3 (2014-12-10)
19 | ------------------
20 | 


--------------------------------------------------------------------------------
/range_sensor_layer/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 2.8.3)
 2 | project(range_sensor_layer)
 3 | set_directory_properties(PROPERTIES COMPILE_OPTIONS "-Wall;-Werror")
 4 | 
 5 | find_package(catkin REQUIRED COMPONENTS
 6 |     angles
 7 |     costmap_2d
 8 |     dynamic_reconfigure
 9 |     geometry_msgs
10 |     pluginlib
11 |     roscpp
12 |     rospy
13 |     sensor_msgs
14 |     tf2_geometry_msgs
15 | )
16 | 
17 | generate_dynamic_reconfigure_options(cfg/RangeSensorLayer.cfg)
18 | 
19 | catkin_package(
20 |     CATKIN_DEPENDS
21 |         angles
22 |         costmap_2d
23 |         dynamic_reconfigure
24 |         geometry_msgs
25 |         pluginlib
26 |         roscpp
27 |         rospy
28 |         sensor_msgs
29 |         tf2_geometry_msgs
30 |     INCLUDE_DIRS include
31 |     LIBRARIES ${PROJECT_NAME}
32 | )
33 | 
34 | include_directories(include ${catkin_INCLUDE_DIRS})
35 | 
36 | add_library(${PROJECT_NAME} src/range_sensor_layer.cpp)
37 | add_dependencies(${PROJECT_NAME} ${range_sensor_layer_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
38 | target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
39 | 
40 | if(CATKIN_ENABLE_TESTING)
41 |   find_package(catkin REQUIRED COMPONENTS roslint)
42 |   roslint_cpp()
43 |   roslint_add_test()
44 | endif()
45 | 
46 | install(TARGETS ${PROJECT_NAME}
47 |         LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
48 |         ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
49 |         RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
50 | )
51 | 
52 | install(FILES costmap_plugins.xml
53 |         DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
54 | )
55 | 
56 | install(DIRECTORY include/${PROJECT_NAME}/
57 |         DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
58 | )
59 | catkin_install_python(PROGRAMS scripts/send_message.py
60 |                       DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
61 | )
62 | 


--------------------------------------------------------------------------------
/range_sensor_layer/README.md:
--------------------------------------------------------------------------------
1 | # Decay option
2 | 
3 | - use_decay : Allow the layer to use a decay on the pixel marked. By default set at false.
4 | - pixel_decay : Time decay in seconds. By default set at 10.0.
5 | 
6 | 


--------------------------------------------------------------------------------
/range_sensor_layer/cfg/RangeSensorLayer.cfg:
--------------------------------------------------------------------------------
 1 | #!/usr/bin/env python
 2 | PACKAGE = "range_sensor_layer"
 3 | 
 4 | from dynamic_reconfigure.parameter_generator_catkin import *
 5 | 
 6 | gen = ParameterGenerator()
 7 | 
 8 | gen.add('enabled',               bool_t, 0, 'Whether to apply this plugin or not', True)
 9 | gen.add('phi',                 double_t, 0, 'Phi value', 1.2)
10 | gen.add('inflate_cone',        double_t, 0, 'Inflate the triangular area covered by the sensor (percentage)', 1, 0, 1)
11 | gen.add('no_readings_timeout', double_t, 0, 'No Readings Timeout', 0.0, 0.0)
12 | gen.add('clear_threshold',     double_t, 0, 'Probability below which cells are marked as free', 0.2, 0.0, 1.0)
13 | gen.add('mark_threshold',      double_t, 0, 'Probability above which cells are marked as occupied', 0.8, 0.0, 1.0)
14 | gen.add('clear_on_max_reading',  bool_t, 0, 'Clear on max reading', False)
15 | 
16 | exit(gen.generate(PACKAGE, PACKAGE, "RangeSensorLayer"))
17 | 


--------------------------------------------------------------------------------
/range_sensor_layer/costmap_plugins.xml:
--------------------------------------------------------------------------------
1 | <library path="lib/librange_sensor_layer">
2 |   <class type="range_sensor_layer::RangeSensorLayer" base_class_type="costmap_2d::Layer">
3 |     <description>A range-sensor (sonar, IR) based obstacle layer for costmap_2d</description>
4 |   </class>
5 | </library>
6 | 


--------------------------------------------------------------------------------
/range_sensor_layer/include/range_sensor_layer/range_sensor_layer.h:
--------------------------------------------------------------------------------
  1 | // Copyright 2018 David V. Lu!!
  2 | #ifndef RANGE_SENSOR_LAYER_RANGE_SENSOR_LAYER_H_
  3 | #define RANGE_SENSOR_LAYER_RANGE_SENSOR_LAYER_H_
  4 | #include <ros/ros.h>
  5 | #include <costmap_2d/costmap_layer.h>
  6 | #include <costmap_2d/layered_costmap.h>
  7 | #include <sensor_msgs/Range.h>
  8 | #include <range_sensor_layer/RangeSensorLayerConfig.h>
  9 | #include <dynamic_reconfigure/server.h>
 10 | #include <list>
 11 | #include <map>
 12 | #include <string>
 13 | #include <utility>
 14 | #include <vector>
 15 | #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 16 | 
 17 | namespace range_sensor_layer
 18 | {
 19 | 
 20 | class RangeSensorLayer : public costmap_2d::CostmapLayer
 21 | {
 22 | public:
 23 |   enum InputSensorType
 24 |   {
 25 |     VARIABLE,
 26 |     FIXED,
 27 |     ALL
 28 |   };
 29 | 
 30 |   RangeSensorLayer();
 31 | 
 32 |   virtual void onInitialize();
 33 |   virtual void updateBounds(double robot_x, double robot_y, double robot_yaw,
 34 |                             double* min_x, double* min_y, double* max_x, double* max_y);
 35 |   virtual void updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
 36 |   virtual void reset();
 37 |   virtual void deactivate();
 38 |   virtual void activate();
 39 | 
 40 | private:
 41 |   void reconfigureCB(range_sensor_layer::RangeSensorLayerConfig &config, uint32_t level);
 42 | 
 43 |   void bufferIncomingRangeMsg(const sensor_msgs::RangeConstPtr& range_message);
 44 |   void processRangeMsg(sensor_msgs::Range& range_message);
 45 |   void processFixedRangeMsg(sensor_msgs::Range& range_message);
 46 |   void processVariableRangeMsg(sensor_msgs::Range& range_message);
 47 | 
 48 |   void resetRange();
 49 |   void updateCostmap();
 50 |   void updateCostmap(sensor_msgs::Range& range_message, bool clear_sensor_cone);
 51 |   void removeOutdatedReadings();
 52 | 
 53 |   double gamma(double theta);
 54 |   double delta(double phi);
 55 |   double sensor_model(double r, double phi, double theta);
 56 | 
 57 |   void get_deltas(double angle, double *dx, double *dy);
 58 |   void update_cell(double ox, double oy, double ot, double r, double nx, double ny, bool clear);
 59 | 
 60 |   double to_prob(unsigned char c)
 61 |   {
 62 |     return static_cast<double>(c) / costmap_2d::LETHAL_OBSTACLE;
 63 |   }
 64 |   unsigned char to_cost(double p)
 65 |   {
 66 |     return static_cast<unsigned char>(p * costmap_2d::LETHAL_OBSTACLE);
 67 |   }
 68 | 
 69 |   boost::function<void(sensor_msgs::Range& range_message)> processRangeMessageFunc_;
 70 |   boost::mutex range_message_mutex_;
 71 |   std::list<sensor_msgs::Range> range_msgs_buffer_;
 72 |   std::map<std::pair<unsigned int, unsigned int>, double> marked_point_history_;
 73 | 
 74 |   double max_angle_, phi_v_;
 75 |   double inflate_cone_;
 76 |   std::string global_frame_;
 77 | 
 78 |   double clear_threshold_, mark_threshold_;
 79 |   bool clear_on_max_reading_;
 80 | 
 81 |   double no_readings_timeout_;
 82 |   ros::Time last_reading_time_;
 83 |   unsigned int buffered_readings_;
 84 |   std::vector<ros::Subscriber> range_subs_;
 85 |   double min_x_, min_y_, max_x_, max_y_;
 86 | 
 87 |   bool use_decay_;
 88 |   double pixel_decay_;
 89 |   double transform_tolerance_;
 90 | 
 91 |   dynamic_reconfigure::Server<range_sensor_layer::RangeSensorLayerConfig> *dsrv_;
 92 | 
 93 | 
 94 |   float area(int x1, int y1, int x2, int y2, int x3, int y3)
 95 |   {
 96 |     return fabs((x1 * (y2 - y3) + x2 * (y3 - y1) + x3 * (y1 - y2)) / 2.0);
 97 |   };
 98 | 
 99 |   int orient2d(int Ax, int Ay, int Bx, int By, int Cx, int Cy)
100 |   {
101 |     return (Bx - Ax) * (Cy - Ay) - (By - Ay) * (Cx - Ax);
102 |   };
103 | };
104 | }  // namespace range_sensor_layer
105 | #endif  // RANGE_SENSOR_LAYER_RANGE_SENSOR_LAYER_H
106 | 


--------------------------------------------------------------------------------
/range_sensor_layer/package.xml:
--------------------------------------------------------------------------------
 1 | <package format="2">
 2 |   <name>range_sensor_layer</name>
 3 |   <version>0.5.0</version>
 4 |   <description>
 5 |      Navigation Layer for Range sensors like sonar and IR
 6 |   </description>
 7 | 
 8 |   <maintainer email="davidvlu@gmail.com">David V. Lu!!</maintainer>
 9 |   <author>David!!</author>
10 | 
11 |   <license>BSD</license>
12 | 
13 |   <url>http://wiki.ros.org/range_sensor_layer</url>
14 | 
15 |   <buildtool_depend>catkin</buildtool_depend>
16 |   <depend>angles</depend>
17 |   <depend>costmap_2d</depend>
18 |   <depend>dynamic_reconfigure</depend>
19 |   <depend>geometry_msgs</depend>
20 |   <depend>pluginlib</depend>
21 |   <depend>roscpp</depend>
22 |   <depend>rospy</depend>
23 |   <depend>sensor_msgs</depend>
24 |   <depend>tf2_geometry_msgs</depend>
25 |   <test_depend>roslint</test_depend>
26 | 
27 |   <export>
28 |     <costmap_2d plugin="${prefix}/costmap_plugins.xml"/>
29 |   </export>
30 | </package>
31 | 


--------------------------------------------------------------------------------
/range_sensor_layer/scripts/send_message.py:
--------------------------------------------------------------------------------
 1 | import rospy
 2 | from sensor_msgs.msg import Range
 3 | 
 4 | rospy.init_node('sender')
 5 | r = Range()
 6 | 
 7 | r.header.frame_id = '/base_laser_link'
 8 | r.field_of_view = 25*3.14/180
 9 | r.max_range = 100
10 | r.range = 1
11 | 
12 | pub = rospy.Publisher('/sonar', Range)
13 | 
14 | while not rospy.is_shutdown():
15 |     r.header.stamp = rospy.Time.now()
16 |     pub.publish(r)
17 | 
18 |     rospy.sleep(5)
19 | 


--------------------------------------------------------------------------------
/range_sensor_layer/src/range_sensor_layer.cpp:
--------------------------------------------------------------------------------
  1 | // Copyright 2018 David V. Lu!!
  2 | #include <range_sensor_layer/range_sensor_layer.h>
  3 | #include <boost/algorithm/string.hpp>
  4 | #include <geometry_msgs/PointStamped.h>
  5 | #include <pluginlib/class_list_macros.h>
  6 | #include <angles/angles.h>
  7 | #include <algorithm>
  8 | #include <list>
  9 | #include <limits>
 10 | #include <map>
 11 | #include <string>
 12 | #include <utility>
 13 | 
 14 | PLUGINLIB_EXPORT_CLASS(range_sensor_layer::RangeSensorLayer, costmap_2d::Layer)
 15 | 
 16 | using costmap_2d::NO_INFORMATION;
 17 | 
 18 | namespace range_sensor_layer
 19 | {
 20 | 
 21 | RangeSensorLayer::RangeSensorLayer() {}
 22 | 
 23 | void RangeSensorLayer::onInitialize()
 24 | {
 25 |   ros::NodeHandle nh("~/" + name_);
 26 |   current_ = true;
 27 |   buffered_readings_ = 0;
 28 |   last_reading_time_ = ros::Time::now();
 29 |   default_value_ = to_cost(0.5);
 30 | 
 31 |   matchSize();
 32 |   resetRange();
 33 | 
 34 |   // Default topic names list contains a single topic: /sonar
 35 |   // We use the XmlRpcValue constructor that takes a XML string and reading start offset
 36 |   const char* xml = "<value><array><data><value>/sonar</value></data></array></value>";
 37 |   int zero_offset = 0;
 38 |   std::string topics_ns;
 39 |   XmlRpc::XmlRpcValue topic_names(xml, &zero_offset);
 40 | 
 41 |   nh.param("ns", topics_ns, std::string());
 42 |   nh.param("topics", topic_names, topic_names);
 43 | 
 44 |   InputSensorType input_sensor_type = ALL;
 45 |   std::string sensor_type_name;
 46 |   nh.param("input_sensor_type", sensor_type_name, std::string("ALL"));
 47 | 
 48 |   nh.param("use_decay", use_decay_, false);
 49 |   nh.param("pixel_decay", pixel_decay_, 10.0);
 50 |   nh.param("transform_tolerance_", transform_tolerance_, 0.3);
 51 | 
 52 |   boost::to_upper(sensor_type_name);
 53 |   ROS_INFO("%s: %s as input_sensor_type given", name_.c_str(), sensor_type_name.c_str());
 54 | 
 55 |   if (sensor_type_name == "VARIABLE")
 56 |     input_sensor_type = VARIABLE;
 57 |   else if (sensor_type_name == "FIXED")
 58 |     input_sensor_type = FIXED;
 59 |   else if (sensor_type_name == "ALL")
 60 |     input_sensor_type = ALL;
 61 |   else
 62 |   {
 63 |     ROS_ERROR("%s: Invalid input sensor type: %s", name_.c_str(), sensor_type_name.c_str());
 64 |   }
 65 | 
 66 |   // Validate topic names list: it must be a (normally non-empty) list of strings
 67 |   if ((topic_names.valid() == false) || (topic_names.getType() != XmlRpc::XmlRpcValue::TypeArray))
 68 |   {
 69 |     ROS_ERROR("Invalid topic names list: it must be a non-empty list of strings");
 70 |     return;
 71 |   }
 72 | 
 73 |   if (topic_names.size() < 1)
 74 |   {
 75 |     // This could be an error, but I keep it as it can be useful for debug
 76 |     ROS_WARN("Empty topic names list: range sensor layer will have no effect on costmap");
 77 |   }
 78 | 
 79 |   // Traverse the topic names list subscribing to all of them with the same callback method
 80 |   for (int i = 0; i < topic_names.size(); i++)
 81 |   {
 82 |     if (topic_names[i].getType() != XmlRpc::XmlRpcValue::TypeString)
 83 |     {
 84 |       ROS_WARN("Invalid topic names list: element %d is not a string, so it will be ignored", i);
 85 |     }
 86 |     else
 87 |     {
 88 |       std::string topic_name(topics_ns);
 89 |       if ((topic_name.size() > 0) && (topic_name.at(topic_name.size() - 1) != '/'))
 90 |         topic_name += "/";
 91 |       topic_name += static_cast<std::string>(topic_names[i]);
 92 | 
 93 |       if (input_sensor_type == VARIABLE)
 94 |         processRangeMessageFunc_ = boost::bind(&RangeSensorLayer::processVariableRangeMsg, this, _1);
 95 |       else if (input_sensor_type == FIXED)
 96 |         processRangeMessageFunc_ = boost::bind(&RangeSensorLayer::processFixedRangeMsg, this, _1);
 97 |       else if (input_sensor_type == ALL)
 98 |         processRangeMessageFunc_ = boost::bind(&RangeSensorLayer::processRangeMsg, this, _1);
 99 |       else
100 |       {
101 |         ROS_ERROR(
102 |           "%s: Invalid input sensor type: %s. Did you make a new type and forgot to choose the subscriber for it?",
103 |           name_.c_str(), sensor_type_name.c_str());
104 |       }
105 | 
106 |       range_subs_.push_back(nh.subscribe(topic_name, 100, &RangeSensorLayer::bufferIncomingRangeMsg, this));
107 | 
108 |       ROS_INFO("RangeSensorLayer: subscribed to topic %s", range_subs_.back().getTopic().c_str());
109 |     }
110 |   }
111 | 
112 |   dsrv_ = new dynamic_reconfigure::Server<range_sensor_layer::RangeSensorLayerConfig>(nh);
113 |   dynamic_reconfigure::Server<range_sensor_layer::RangeSensorLayerConfig>::CallbackType cb =
114 |     boost::bind(&RangeSensorLayer::reconfigureCB, this, _1, _2);
115 |   dsrv_->setCallback(cb);
116 |   global_frame_ = layered_costmap_->getGlobalFrameID();
117 | }
118 | 
119 | 
120 | double RangeSensorLayer::gamma(double theta)
121 | {
122 |   if (fabs(theta) > max_angle_)
123 |     return 0.0;
124 |   else
125 |     return 1 - pow(theta / max_angle_, 2);
126 | }
127 | 
128 | double RangeSensorLayer::delta(double phi)
129 | {
130 |   return 1 - (1 + tanh(2 * (phi - phi_v_))) / 2;
131 | }
132 | 
133 | void RangeSensorLayer::get_deltas(double angle, double *dx, double *dy)
134 | {
135 |   double ta = tan(angle);
136 |   if (ta == 0)
137 |     *dx = 0;
138 |   else
139 |     *dx = resolution_ / ta;
140 | 
141 |   *dx = copysign(*dx, cos(angle));
142 |   *dy = copysign(resolution_, sin(angle));
143 | }
144 | 
145 | double RangeSensorLayer::sensor_model(double r, double phi, double theta)
146 | {
147 |   double lbda = delta(phi) * gamma(theta);
148 | 
149 |   double delta = resolution_;
150 | 
151 |   if (phi >= 0.0 && phi < r - 2 * delta * r)
152 |     return (1 - lbda) * (0.5);
153 |   else if (phi < r - delta * r)
154 |     return lbda * 0.5 * pow((phi - (r - 2 * delta * r)) / (delta * r), 2) + (1 - lbda) * .5;
155 |   else if (phi < r + delta * r)
156 |   {
157 |     double J = (r - phi) / (delta * r);
158 |     return lbda * ((1 - (0.5) * pow(J, 2)) - 0.5) + 0.5;
159 |   }
160 |   else
161 |     return 0.5;
162 | }
163 | 
164 | 
165 | void RangeSensorLayer::reconfigureCB(range_sensor_layer::RangeSensorLayerConfig &config, uint32_t level)
166 | {
167 |   phi_v_ = config.phi;
168 |   inflate_cone_ = config.inflate_cone;
169 |   no_readings_timeout_ = config.no_readings_timeout;
170 |   clear_threshold_ = config.clear_threshold;
171 |   mark_threshold_ = config.mark_threshold;
172 |   clear_on_max_reading_ = config.clear_on_max_reading;
173 | 
174 |   if (enabled_ != config.enabled)
175 |   {
176 |     enabled_ = config.enabled;
177 |     current_ = false;
178 |   }
179 | }
180 | 
181 | void RangeSensorLayer::bufferIncomingRangeMsg(const sensor_msgs::RangeConstPtr& range_message)
182 | {
183 |   boost::mutex::scoped_lock lock(range_message_mutex_);
184 |   range_msgs_buffer_.push_back(*range_message);
185 | }
186 | 
187 | void RangeSensorLayer::updateCostmap()
188 | {
189 |   std::list<sensor_msgs::Range> range_msgs_buffer_copy;
190 | 
191 |   range_message_mutex_.lock();
192 |   range_msgs_buffer_copy = std::list<sensor_msgs::Range>(range_msgs_buffer_);
193 |   range_msgs_buffer_.clear();
194 |   range_message_mutex_.unlock();
195 | 
196 |   for (std::list<sensor_msgs::Range>::iterator range_msgs_it = range_msgs_buffer_copy.begin();
197 |        range_msgs_it != range_msgs_buffer_copy.end(); range_msgs_it++)
198 |   {
199 |     processRangeMessageFunc_(*range_msgs_it);
200 |   }
201 | }
202 | 
203 | void RangeSensorLayer::processRangeMsg(sensor_msgs::Range& range_message)
204 | {
205 |   if (range_message.min_range == range_message.max_range)
206 |     processFixedRangeMsg(range_message);
207 |   else
208 |     processVariableRangeMsg(range_message);
209 | }
210 | 
211 | void RangeSensorLayer::processFixedRangeMsg(sensor_msgs::Range& range_message)
212 | {
213 |   if (!std::isinf(range_message.range))
214 |   {
215 |     ROS_ERROR_THROTTLE(1.0,
216 |                        "Fixed distance ranger (min_range == max_range) in frame %s sent invalid value. "
217 |                        "Only -Inf (== object detected) and Inf (== no object detected) are valid.",
218 |                        range_message.header.frame_id.c_str());
219 |     return;
220 |   }
221 | 
222 |   bool clear_sensor_cone = false;
223 | 
224 |   if (range_message.range > 0)  // +inf
225 |   {
226 |     if (!clear_on_max_reading_)
227 |       return;  // no clearing at all
228 | 
229 |     clear_sensor_cone = true;
230 |   }
231 | 
232 |   range_message.range = range_message.min_range;
233 | 
234 |   updateCostmap(range_message, clear_sensor_cone);
235 | }
236 | 
237 | void RangeSensorLayer::processVariableRangeMsg(sensor_msgs::Range& range_message)
238 | {
239 |   if (range_message.range < range_message.min_range || range_message.range > range_message.max_range)
240 |     return;
241 | 
242 |   bool clear_sensor_cone = false;
243 | 
244 |   if (range_message.range == range_message.max_range && clear_on_max_reading_)
245 |     clear_sensor_cone = true;
246 | 
247 |   updateCostmap(range_message, clear_sensor_cone);
248 | }
249 | 
250 | void RangeSensorLayer::updateCostmap(sensor_msgs::Range& range_message, bool clear_sensor_cone)
251 | {
252 |   max_angle_ = range_message.field_of_view / 2;
253 | 
254 |   geometry_msgs::PointStamped in, out;
255 |   in.header.stamp = range_message.header.stamp;
256 |   in.header.frame_id = range_message.header.frame_id;
257 | 
258 |   if (!tf_->canTransform(global_frame_, in.header.frame_id, in.header.stamp, ros::Duration(transform_tolerance_)))
259 |   {
260 |     ROS_ERROR_THROTTLE(1.0, "Range sensor layer can't transform from %s to %s at %f",
261 |                        global_frame_.c_str(), in.header.frame_id.c_str(),
262 |                        in.header.stamp.toSec());
263 |     return;
264 |   }
265 | 
266 |   tf_->transform(in, out, global_frame_);
267 | 
268 |   double ox = out.point.x, oy = out.point.y;
269 | 
270 |   in.point.x = range_message.range;
271 | 
272 |   tf_->transform(in, out, global_frame_);
273 | 
274 |   double tx = out.point.x, ty = out.point.y;
275 | 
276 |   // calculate target props
277 |   double dx = tx - ox, dy = ty - oy, theta = atan2(dy, dx), d = sqrt(dx * dx + dy * dy);
278 | 
279 |   // Integer Bounds of Update
280 |   int bx0, by0, bx1, by1;
281 | 
282 |   // Triangle that will be really updated; the other cells within bounds are ignored
283 |   // This triangle is formed by the origin and left and right sides of sonar cone
284 |   int Ox, Oy, Ax, Ay, Bx, By;
285 | 
286 |   // Bounds includes the origin
287 |   worldToMapNoBounds(ox, oy, Ox, Oy);
288 |   bx1 = bx0 = Ox;
289 |   by1 = by0 = Oy;
290 |   touch(ox, oy, &min_x_, &min_y_, &max_x_, &max_y_);
291 | 
292 |   // Update Map with Target Point
293 |   unsigned int aa, ab;
294 |   if (worldToMap(tx, ty, aa, ab))
295 |   {
296 |     setCost(aa, ab, 233);
297 |     touch(tx, ty, &min_x_, &min_y_, &max_x_, &max_y_);
298 |   }
299 | 
300 |   double mx, my;
301 | 
302 |   // Update left side of sonar cone
303 |   mx = ox + cos(theta - max_angle_) * d * 1.2;
304 |   my = oy + sin(theta - max_angle_) * d * 1.2;
305 |   worldToMapNoBounds(mx, my, Ax, Ay);
306 |   bx0 = std::min(bx0, Ax);
307 |   bx1 = std::max(bx1, Ax);
308 |   by0 = std::min(by0, Ay);
309 |   by1 = std::max(by1, Ay);
310 |   touch(mx, my, &min_x_, &min_y_, &max_x_, &max_y_);
311 | 
312 |   // Update right side of sonar cone
313 |   mx = ox + cos(theta + max_angle_) * d * 1.2;
314 |   my = oy + sin(theta + max_angle_) * d * 1.2;
315 | 
316 |   worldToMapNoBounds(mx, my, Bx, By);
317 |   bx0 = std::min(bx0, Bx);
318 |   bx1 = std::max(bx1, Bx);
319 |   by0 = std::min(by0, By);
320 |   by1 = std::max(by1, By);
321 |   touch(mx, my, &min_x_, &min_y_, &max_x_, &max_y_);
322 | 
323 |   // Limit Bounds to Grid
324 |   bx0 = std::max(0, bx0);
325 |   by0 = std::max(0, by0);
326 |   bx1 = std::min(static_cast<int>(size_x_), bx1);
327 |   by1 = std::min(static_cast<int>(size_y_), by1);
328 | 
329 |   for (unsigned int x = bx0; x <= (unsigned int)bx1; x++)
330 |   {
331 |     for (unsigned int y = by0; y <= (unsigned int)by1; y++)
332 |     {
333 |       bool update_xy_cell = true;
334 | 
335 |       // Unless inflate_cone_ is set to 100 %, we update cells only within the (partially inflated) sensor cone,
336 |       // projected on the costmap as a triangle. 0 % corresponds to just the triangle, but if your sensor fov is
337 |       // very narrow, the covered area can become zero due to cell discretization. See wiki description for more
338 |       // details
339 |       if (inflate_cone_ < 1.0)
340 |       {
341 |         // Determine barycentric coordinates
342 |         int w0 = orient2d(Ax, Ay, Bx, By, x, y);
343 |         int w1 = orient2d(Bx, By, Ox, Oy, x, y);
344 |         int w2 = orient2d(Ox, Oy, Ax, Ay, x, y);
345 | 
346 |         // Barycentric coordinates inside area threshold; this is not mathematically sound at all, but it works!
347 |         float bcciath = -inflate_cone_ * area(Ax, Ay, Bx, By, Ox, Oy);
348 |         update_xy_cell = w0 >= bcciath && w1 >= bcciath && w2 >= bcciath;
349 |       }
350 | 
351 |       if (update_xy_cell)
352 |       {
353 |         double wx, wy;
354 |         mapToWorld(x, y, wx, wy);
355 |         update_cell(ox, oy, theta, range_message.range, wx, wy, clear_sensor_cone);
356 |       }
357 |     }
358 |   }
359 | 
360 |   buffered_readings_++;
361 |   last_reading_time_ = ros::Time::now();
362 |   if (use_decay_)
363 |     removeOutdatedReadings();
364 | }
365 | 
366 | void RangeSensorLayer::removeOutdatedReadings()
367 | {
368 |   std::map<std::pair<unsigned int, unsigned int>, double>::iterator it_map;
369 | 
370 |   double removal_time = last_reading_time_.toSec() - pixel_decay_;
371 |   for (it_map = marked_point_history_.begin() ; it_map != marked_point_history_.end() ; it_map++ )
372 |   {
373 |     if (it_map->second < removal_time)
374 |     {
375 |       marked_point_history_.erase(it_map);
376 |       setCost(std::get<0>(it_map->first), std::get<1>(it_map->first), costmap_2d::FREE_SPACE);
377 |     }
378 |   }
379 | }
380 | 
381 | void RangeSensorLayer::update_cell(double ox, double oy, double ot, double r, double nx, double ny, bool clear)
382 | {
383 |   unsigned int x, y;
384 |   if (worldToMap(nx, ny, x, y))
385 |   {
386 |     double dx = nx - ox, dy = ny - oy;
387 |     double theta = atan2(dy, dx) - ot;
388 |     theta = angles::normalize_angle(theta);
389 |     double phi = sqrt(dx * dx + dy * dy);
390 |     double sensor = 0.0;
391 |     if (!clear)
392 |       sensor = sensor_model(r, phi, theta);
393 |     double prior = to_prob(getCost(x, y));
394 |     double prob_occ = sensor * prior;
395 |     double prob_not = (1 - sensor) * (1 - prior);
396 |     double new_prob = prob_occ / (prob_occ + prob_not);
397 | 
398 |     ROS_DEBUG("%f %f | %f %f = %f", dx, dy, theta, phi, sensor);
399 |     ROS_DEBUG("%f | %f %f | %f", prior, prob_occ, prob_not, new_prob);
400 |     unsigned char c = to_cost(new_prob);
401 | 
402 |     setCost(x, y, c);
403 |     if (use_decay_)
404 |     {
405 |       std::pair<unsigned int, unsigned int> coordinate_pair(x, y);
406 |       // If the point has a score high enough to be marked in the costmap, we add it's time to the marked_point_history
407 |       if (c > to_cost(mark_threshold_))
408 |         marked_point_history_[coordinate_pair] = last_reading_time_.toSec();
409 |       // If the point score is not high enough, we try to find it in the mark history point.
410 |       // In the case we find it in the marked_point_history
411 |       // we clear it from the map so we won't checked already cleared point
412 |       else if (c < to_cost(clear_threshold_))
413 |       {
414 |         std::map<std::pair<unsigned int, unsigned int>, double>::iterator it_clear;
415 |         it_clear = marked_point_history_.find(coordinate_pair);
416 |         if (it_clear != marked_point_history_.end())
417 |           marked_point_history_.erase(it_clear);
418 |       }
419 |     }
420 |   }
421 | }
422 | 
423 | void RangeSensorLayer::resetRange()
424 | {
425 |   min_x_ = min_y_ =  std::numeric_limits<double>::max();
426 |   max_x_ = max_y_ = -std::numeric_limits<double>::max();
427 | }
428 | 
429 | void RangeSensorLayer::updateBounds(double robot_x, double robot_y, double robot_yaw,
430 |                                     double* min_x, double* min_y, double* max_x, double* max_y)
431 | {
432 |   if (layered_costmap_->isRolling())
433 |     updateOrigin(robot_x - getSizeInMetersX() / 2, robot_y - getSizeInMetersY() / 2);
434 | 
435 |   updateCostmap();
436 | 
437 |   *min_x = std::min(*min_x, min_x_);
438 |   *min_y = std::min(*min_y, min_y_);
439 |   *max_x = std::max(*max_x, max_x_);
440 |   *max_y = std::max(*max_y, max_y_);
441 | 
442 |   resetRange();
443 | 
444 |   if (!enabled_)
445 |   {
446 |     current_ = true;
447 |     return;
448 |   }
449 | 
450 |   if (buffered_readings_ == 0)
451 |   {
452 |     if (no_readings_timeout_ > 0.0 &&
453 |         (ros::Time::now() - last_reading_time_).toSec() > no_readings_timeout_)
454 |     {
455 |       ROS_WARN_THROTTLE(2.0, "No range readings received for %.2f seconds, " \
456 |                         "while expected at least every %.2f seconds.",
457 |                         (ros::Time::now() - last_reading_time_).toSec(), no_readings_timeout_);
458 |       current_ = false;
459 |     }
460 |   }
461 | }
462 | 
463 | void RangeSensorLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
464 | {
465 |   if (!enabled_)
466 |     return;
467 | 
468 |   unsigned char* master_array = master_grid.getCharMap();
469 |   unsigned int span = master_grid.getSizeInCellsX();
470 |   unsigned char clear = to_cost(clear_threshold_), mark = to_cost(mark_threshold_);
471 | 
472 |   for (int j = min_j; j < max_j; j++)
473 |   {
474 |     unsigned int it = j * span + min_i;
475 |     for (int i = min_i; i < max_i; i++)
476 |     {
477 |       unsigned char prob = costmap_[it];
478 |       unsigned char current;
479 |       if (prob == costmap_2d::NO_INFORMATION)
480 |       {
481 |         it++;
482 |         continue;
483 |       }
484 |       else if (prob > mark)
485 |         current = costmap_2d::LETHAL_OBSTACLE;
486 |       else if (prob < clear)
487 |         current = costmap_2d::FREE_SPACE;
488 |       else
489 |       {
490 |         it++;
491 |         continue;
492 |       }
493 | 
494 |       unsigned char old_cost = master_array[it];
495 | 
496 |       if (old_cost == NO_INFORMATION || old_cost < current)
497 |         master_array[it] = current;
498 |       it++;
499 |     }
500 |   }
501 | 
502 |   buffered_readings_ = 0;
503 |   current_ = true;
504 | }
505 | 
506 | void RangeSensorLayer::reset()
507 | {
508 |   ROS_DEBUG("Reseting range sensor layer...");
509 |   deactivate();
510 |   resetMaps();
511 |   current_ = true;
512 |   activate();
513 | }
514 | 
515 | void RangeSensorLayer::deactivate()
516 | {
517 |   range_msgs_buffer_.clear();
518 | }
519 | 
520 | void RangeSensorLayer::activate()
521 | {
522 |   range_msgs_buffer_.clear();
523 | }
524 | 
525 | }  // namespace range_sensor_layer
526 | 


--------------------------------------------------------------------------------
/social_navigation_layers/CHANGELOG.rst:
--------------------------------------------------------------------------------
 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 2 | Changelog for package social_navigation_layers
 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 4 | 
 5 | 0.2.3 (2014-12-10)
 6 | ------------------
 7 | 
 8 | 0.4.0 (2018-08-31)
 9 | ------------------
10 | * Clean up with roscompile
11 | * Contributors: David V. Lu
12 | 
13 | 0.2.4 (2014-12-10)
14 | ------------------
15 | 
16 | 1.0.4 (2014-07-09)
17 | ------------------
18 | * merging Social Navigation Layer into people
19 | * Contributors: David Lu!!
20 | 
21 | 1.0.3 (2014-03-01)
22 | ------------------
23 | 
24 | 1.0.2 (2014-02-28)
25 | ------------------
26 | 
27 | 1.0.1 (2014-02-27)
28 | ------------------
29 | 


--------------------------------------------------------------------------------
/social_navigation_layers/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 2.8.3)
 2 | project(social_navigation_layers)
 3 | set_directory_properties(PROPERTIES COMPILE_OPTIONS "-Wall;-Werror")
 4 | 
 5 | find_package(catkin REQUIRED COMPONENTS
 6 |   angles
 7 |   costmap_2d
 8 |   dynamic_reconfigure
 9 |   geometry_msgs
10 |   people_msgs
11 |   pluginlib
12 |   roscpp
13 | )
14 | 
15 | generate_dynamic_reconfigure_options(
16 |   cfg/ProxemicLayer.cfg
17 | )
18 | 
19 | catkin_package(
20 |     CATKIN_DEPENDS angles costmap_2d dynamic_reconfigure geometry_msgs people_msgs pluginlib roscpp
21 |     INCLUDE_DIRS include
22 |     LIBRARIES social_layers
23 | )
24 | 
25 | include_directories(
26 |   include ${catkin_INCLUDE_DIRS}
27 | )
28 | 
29 | ## add cpp library
30 | add_library(social_layers
31 |             src/social_layer.cpp
32 |             src/proxemic_layer.cpp
33 |             src/passing_layer.cpp
34 | )
35 | add_dependencies(social_layers ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
36 | target_link_libraries(
37 |     social_layers ${catkin_LIBRARIES}
38 | )
39 | 
40 | if(CATKIN_ENABLE_TESTING)
41 |   find_package(catkin REQUIRED COMPONENTS roslint)
42 |   roslint_cpp()
43 |   roslint_add_test()
44 | endif()
45 | 
46 | install(FILES costmap_plugins.xml
47 |         DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
48 | )
49 | 
50 | install(TARGETS social_layers
51 |         LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
52 |         ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
53 |         RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
54 | )
55 | 
56 | install(
57 |     DIRECTORY include/${PROJECT_NAME}/
58 |     DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
59 | )
60 | 


--------------------------------------------------------------------------------
/social_navigation_layers/cfg/ProxemicLayer.cfg:
--------------------------------------------------------------------------------
 1 | #!/usr/bin/env python
 2 | 
 3 | PACKAGE='social_navigation_layers'
 4 | 
 5 | import sys
 6 | from dynamic_reconfigure.parameter_generator_catkin import *
 7 | 
 8 | gen = ParameterGenerator()
 9 | 
10 | gen.add("enabled",    bool_t,   0, "Whether to apply this plugin or not ",             True)
11 | gen.add("cutoff",     double_t, 0, "Smallest value to publish on costmap adjustments", 10.0, 1.0, 254.0)
12 | gen.add("amplitude",  double_t, 0, "Amplitude of adjustments at peak",                 77.0, 0.0, 254.0)
13 | gen.add("covariance", double_t, 0, "Covariance of adjustments",                        0.25, 0.0,   5.0)
14 | gen.add("factor",     double_t, 0, "Factor with which to scale the velocity",           5.0, 0.0,  20.0)
15 | gen.add("keep_time",  double_t, 0, "Pause before clearing leg list",                   0.75, 0.0,   2.0)
16 | exit(gen.generate(PACKAGE, "social_navigation_layers", "ProxemicLayer"))
17 | 


--------------------------------------------------------------------------------
/social_navigation_layers/costmap_plugins.xml:
--------------------------------------------------------------------------------
1 | <library path="lib/libsocial_layers">
2 |   <class type="social_navigation_layers::ProxemicLayer" base_class_type="costmap_2d::Layer">
3 |     <description>Uses people information to change the costmap</description>
4 |   </class>
5 |   <class type="social_navigation_layers::PassingLayer" base_class_type="costmap_2d::Layer">
6 |     <description>Uses people information to change the costmap</description>
7 |   </class>
8 | </library>
9 | 


--------------------------------------------------------------------------------
/social_navigation_layers/include/social_navigation_layers/proxemic_layer.h:
--------------------------------------------------------------------------------
 1 | // Copyright 2018 David V. Lu!!
 2 | #ifndef SOCIAL_NAVIGATION_LAYERS_PROXEMIC_LAYER_H
 3 | #define SOCIAL_NAVIGATION_LAYERS_PROXEMIC_LAYER_H
 4 | #include <ros/ros.h>
 5 | #include <social_navigation_layers/social_layer.h>
 6 | #include <dynamic_reconfigure/server.h>
 7 | #include <social_navigation_layers/ProxemicLayerConfig.h>
 8 | 
 9 | double gaussian(double x, double y, double x0, double y0, double A, double varx, double vary, double skew);
10 | double get_radius(double cutoff, double A, double var);
11 | 
12 | namespace social_navigation_layers
13 | {
14 | class ProxemicLayer : public SocialLayer
15 | {
16 | public:
17 |   ProxemicLayer()
18 |   {
19 |     layered_costmap_ = NULL;
20 |   }
21 | 
22 |   virtual void onInitialize();
23 |   virtual void updateBoundsFromPeople(double* min_x, double* min_y, double* max_x, double* max_y);
24 |   virtual void updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
25 | 
26 | protected:
27 |   void configure(ProxemicLayerConfig &config, uint32_t level);
28 |   double cutoff_, amplitude_, covar_, factor_;
29 |   dynamic_reconfigure::Server<ProxemicLayerConfig>* server_;
30 |   dynamic_reconfigure::Server<ProxemicLayerConfig>::CallbackType f_;
31 | };
32 | }  // namespace social_navigation_layers
33 | 
34 | #endif  // SOCIAL_NAVIGATION_LAYERS_PROXEMIC_LAYER_H
35 | 


--------------------------------------------------------------------------------
/social_navigation_layers/include/social_navigation_layers/social_layer.h:
--------------------------------------------------------------------------------
 1 | // Copyright 2018 David V. Lu!!
 2 | #ifndef SOCIAL_NAVIGATION_LAYERS_SOCIAL_LAYER_H
 3 | #define SOCIAL_NAVIGATION_LAYERS_SOCIAL_LAYER_H
 4 | #include <ros/ros.h>
 5 | #include <costmap_2d/layer.h>
 6 | #include <costmap_2d/layered_costmap.h>
 7 | #include <people_msgs/People.h>
 8 | #include <boost/thread.hpp>
 9 | #include <list>
10 | 
11 | namespace social_navigation_layers
12 | {
13 | class SocialLayer : public costmap_2d::Layer
14 | {
15 | public:
16 |   SocialLayer()
17 |   {
18 |     layered_costmap_ = NULL;
19 |   }
20 | 
21 |   virtual void onInitialize();
22 |   virtual void updateBounds(double origin_x, double origin_y, double origin_yaw, double* min_x, double* min_y,
23 |                             double* max_x, double* max_y);
24 |   virtual void updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j) = 0;
25 | 
26 |   virtual void updateBoundsFromPeople(double* min_x, double* min_y, double* max_x, double* max_y) = 0;
27 | 
28 |   bool isDiscretized()
29 |   {
30 |     return false;
31 |   }
32 | 
33 | protected:
34 |   void peopleCallback(const people_msgs::People& people);
35 |   ros::Subscriber people_sub_;
36 |   people_msgs::People people_list_;
37 |   std::list<people_msgs::Person> transformed_people_;
38 |   ros::Duration people_keep_time_;
39 |   boost::recursive_mutex lock_;
40 |   bool first_time_;
41 |   double last_min_x_, last_min_y_, last_max_x_, last_max_y_;
42 | };
43 | }  // namespace social_navigation_layers
44 | 
45 | #endif  // SOCIAL_NAVIGATION_LAYERS_SOCIAL_LAYER_H
46 | 


--------------------------------------------------------------------------------
/social_navigation_layers/package.xml:
--------------------------------------------------------------------------------
 1 | <package format="2">
 2 |   <name>social_navigation_layers</name>
 3 |   <version>0.5.0</version>
 4 |   <description>
 5 |   Plugin-based layers for the navigation stack that
 6 |   implement various social navigation contraints, like proxemic distance.
 7 |   </description>
 8 |   <author email="davidvlu@gmail.com">David V. Lu!!</author>
 9 |   <maintainer email="davidvlu@gmail.com">David V. Lu!!</maintainer>
10 |   <license>BSD</license>
11 |   <url>http://ros.org/wiki/social_navigation_layers</url>
12 | 
13 |   <buildtool_depend>catkin</buildtool_depend>
14 |   <depend>angles</depend>
15 |   <depend>costmap_2d</depend>
16 |   <depend>dynamic_reconfigure</depend>
17 |   <depend>geometry_msgs</depend>
18 |   <depend>people_msgs</depend>
19 |   <depend>pluginlib</depend>
20 |   <depend>roscpp</depend>
21 |   <test_depend>roslint</test_depend>
22 | 
23 |   <export>
24 |     <costmap_2d plugin="${prefix}/costmap_plugins.xml"/>
25 |   </export>
26 | 
27 | </package>
28 | 


--------------------------------------------------------------------------------
/social_navigation_layers/src/passing_layer.cpp:
--------------------------------------------------------------------------------
  1 | // Copyright 2018 David V. Lu!!
  2 | #include <social_navigation_layers/proxemic_layer.h>
  3 | #include <geometry_msgs/PointStamped.h>
  4 | #include <pluginlib/class_list_macros.h>
  5 | #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
  6 | #include <angles/angles.h>
  7 | #include <algorithm>
  8 | #include <list>
  9 | #include <string>
 10 | 
 11 | namespace social_navigation_layers
 12 | {
 13 | class PassingLayer : public ProxemicLayer
 14 | {
 15 | public:
 16 |   PassingLayer() : ProxemicLayer()
 17 |   {
 18 |   }
 19 | 
 20 |   virtual void updateBounds(double origin_x, double origin_y, double origin_z, double* min_x, double* min_y,
 21 |                             double* max_x, double* max_y)
 22 |   {
 23 |     boost::recursive_mutex::scoped_lock lock(lock_);
 24 | 
 25 |     std::string global_frame = layered_costmap_->getGlobalFrameID();
 26 |     transformed_people_.clear();
 27 | 
 28 |     for (unsigned int i = 0; i < people_list_.people.size(); i++)
 29 |     {
 30 |       people_msgs::Person& person = people_list_.people[i];
 31 |       people_msgs::Person tpt;
 32 |       geometry_msgs::PointStamped pt, opt;
 33 | 
 34 |       try
 35 |       {
 36 |         pt.point.x = person.position.x;
 37 |         pt.point.y = person.position.y;
 38 |         pt.point.z = person.position.z;
 39 |         pt.header.frame_id = people_list_.header.frame_id;
 40 |         pt.header.stamp = people_list_.header.stamp;
 41 |         tf_->transform(pt, opt, global_frame);
 42 |         tpt.position.x = opt.point.x;
 43 |         tpt.position.y = opt.point.y;
 44 |         tpt.position.z = opt.point.z;
 45 | 
 46 |         pt.point.x += person.velocity.x;
 47 |         pt.point.y += person.velocity.y;
 48 |         pt.point.z += person.velocity.z;
 49 |         tf_->transform(pt, opt, global_frame);
 50 | 
 51 |         tpt.velocity.x = tpt.position.x - opt.point.x;
 52 |         tpt.velocity.y = tpt.position.y - opt.point.y;
 53 |         tpt.velocity.z = tpt.position.z - opt.point.z;
 54 | 
 55 |         transformed_people_.push_back(tpt);
 56 | 
 57 |         double mag = sqrt(pow(tpt.velocity.x, 2) + pow(person.velocity.y, 2));
 58 |         double factor = 1.0 + mag * factor_;
 59 |         double point = get_radius(cutoff_, amplitude_, covar_ * factor);
 60 | 
 61 |         *min_x = std::min(*min_x, tpt.position.x - point);
 62 |         *min_y = std::min(*min_y, tpt.position.y - point);
 63 |         *max_x = std::max(*max_x, tpt.position.x + point);
 64 |         *max_y = std::max(*max_y, tpt.position.y + point);
 65 |       }
 66 |       catch (tf2::LookupException& ex)
 67 |       {
 68 |         ROS_ERROR("No Transform available Error: %s\n", ex.what());
 69 |         continue;
 70 |       }
 71 |       catch (tf2::ConnectivityException& ex)
 72 |       {
 73 |         ROS_ERROR("Connectivity Error: %s\n", ex.what());
 74 |         continue;
 75 |       }
 76 |       catch (tf2::ExtrapolationException& ex)
 77 |       {
 78 |         ROS_ERROR("Extrapolation Error: %s\n", ex.what());
 79 |         continue;
 80 |       }
 81 |     }
 82 |   }
 83 | 
 84 |   virtual void updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
 85 |   {
 86 |     boost::recursive_mutex::scoped_lock lock(lock_);
 87 |     if (!enabled_) return;
 88 | 
 89 |     if (people_list_.people.size() == 0)
 90 |       return;
 91 |     if (cutoff_ >= amplitude_)
 92 |       return;
 93 | 
 94 |     std::list<people_msgs::Person>::iterator p_it;
 95 |     costmap_2d::Costmap2D* costmap = layered_costmap_->getCostmap();
 96 |     double res = costmap->getResolution();
 97 | 
 98 |     for (p_it = transformed_people_.begin(); p_it != transformed_people_.end(); ++p_it)
 99 |     {
100 |       people_msgs::Person person = *p_it;
101 |       double angle = atan2(person.velocity.y, person.velocity.x) + 1.51;
102 |       double mag = sqrt(pow(person.velocity.x, 2) + pow(person.velocity.y, 2));
103 |       double factor = 1.0 + mag * factor_;
104 |       double base = get_radius(cutoff_, amplitude_, covar_);
105 |       double point = get_radius(cutoff_, amplitude_, covar_ * factor);
106 | 
107 |       unsigned int width = std::max(1, static_cast<int>((base + point) / res)),
108 |                    height = std::max(1, static_cast<int>((base + point) / res));
109 | 
110 |       double cx = person.position.x, cy = person.position.y;
111 | 
112 |       double ox, oy;
113 |       if (sin(angle) > 0)
114 |         oy = cy - base;
115 |       else
116 |         oy = cy + (point - base) * sin(angle) - base;
117 | 
118 |       if (cos(angle) >= 0)
119 |         ox = cx - base;
120 |       else
121 |         ox = cx + (point - base) * cos(angle) - base;
122 | 
123 | 
124 |       int dx, dy;
125 |       costmap->worldToMapNoBounds(ox, oy, dx, dy);
126 | 
127 |       int start_x = 0, start_y = 0, end_x = width, end_y = height;
128 |       if (dx < 0)
129 |         start_x = -dx;
130 |       else if (dx + width > costmap->getSizeInCellsX())
131 |         end_x = std::max(0, static_cast<int>(costmap->getSizeInCellsX()) - dx);
132 | 
133 |       if (static_cast<int>(start_x + dx) < min_i)
134 |         start_x = min_i - dx;
135 |       if (static_cast<int>(end_x + dx) > max_i)
136 |         end_x = max_i - dx;
137 | 
138 |       if (dy < 0)
139 |         start_y = -dy;
140 |       else if (dy + height > costmap->getSizeInCellsY())
141 |         end_y = std::max(0, static_cast<int>(costmap->getSizeInCellsY()) - dy);
142 | 
143 |       if (static_cast<int>(start_y + dy) < min_j)
144 |         start_y = min_j - dy;
145 |       if (static_cast<int>(end_y + dy) > max_j)
146 |         end_y = max_j - dy;
147 | 
148 |       double bx = ox + res / 2,
149 |              by = oy + res / 2;
150 |       for (int i = start_x; i < end_x; i++)
151 |       {
152 |         for (int j = start_y; j < end_y; j++)
153 |         {
154 |           unsigned char old_cost = costmap->getCost(i + dx, j + dy);
155 |           if (old_cost == costmap_2d::NO_INFORMATION)
156 |             continue;
157 | 
158 |           double x = bx + i * res, y = by + j * res;
159 |           double ma = atan2(y - cy, x - cx);
160 |           double diff = angles::shortest_angular_distance(angle, ma);
161 |           double a;
162 |           if (fabs(diff) < M_PI / 2)
163 |             a = gaussian(x, y, cx, cy, amplitude_, covar_ * factor, covar_, angle);
164 |           else
165 |             continue;
166 | 
167 |           if (a < cutoff_)
168 |             continue;
169 |           unsigned char cvalue = (unsigned char) a;
170 |           costmap->setCost(i + dx, j + dy, std::max(cvalue, old_cost));
171 |         }
172 |       }
173 |     }
174 |   }
175 | };
176 | };  // namespace social_navigation_layers
177 | 
178 | PLUGINLIB_EXPORT_CLASS(social_navigation_layers::PassingLayer, costmap_2d::Layer)
179 | 


--------------------------------------------------------------------------------
/social_navigation_layers/src/proxemic_layer.cpp:
--------------------------------------------------------------------------------
  1 | // Copyright 2018 David V. Lu!!
  2 | #include <social_navigation_layers/proxemic_layer.h>
  3 | #include <math.h>
  4 | #include <angles/angles.h>
  5 | #include <pluginlib/class_list_macros.h>
  6 | #include <algorithm>
  7 | #include <list>
  8 | PLUGINLIB_EXPORT_CLASS(social_navigation_layers::ProxemicLayer, costmap_2d::Layer)
  9 | 
 10 | using costmap_2d::NO_INFORMATION;
 11 | using costmap_2d::LETHAL_OBSTACLE;
 12 | using costmap_2d::FREE_SPACE;
 13 | 
 14 | double gaussian(double x, double y, double x0, double y0, double A, double varx, double vary, double skew)
 15 | {
 16 |   double dx = x - x0, dy = y - y0;
 17 |   double h = sqrt(dx * dx + dy * dy);
 18 |   double angle = atan2(dy, dx);
 19 |   double mx = cos(angle - skew) * h;
 20 |   double my = sin(angle - skew) * h;
 21 |   double f1 = pow(mx, 2.0) / (2.0 * varx),
 22 |          f2 = pow(my, 2.0) / (2.0 * vary);
 23 |   return A * exp(-(f1 + f2));
 24 | }
 25 | 
 26 | double get_radius(double cutoff, double A, double var)
 27 | {
 28 |   return sqrt(-2 * var * log(cutoff / A));
 29 | }
 30 | 
 31 | 
 32 | namespace social_navigation_layers
 33 | {
 34 | void ProxemicLayer::onInitialize()
 35 | {
 36 |   SocialLayer::onInitialize();
 37 |   ros::NodeHandle nh("~/" + name_), g_nh;
 38 |   server_ = new dynamic_reconfigure::Server<ProxemicLayerConfig>(nh);
 39 |   f_ = boost::bind(&ProxemicLayer::configure, this, _1, _2);
 40 |   server_->setCallback(f_);
 41 | }
 42 | 
 43 | void ProxemicLayer::updateBoundsFromPeople(double* min_x, double* min_y, double* max_x, double* max_y)
 44 | {
 45 |   std::list<people_msgs::Person>::iterator p_it;
 46 | 
 47 |   for (p_it = transformed_people_.begin(); p_it != transformed_people_.end(); ++p_it)
 48 |   {
 49 |     people_msgs::Person person = *p_it;
 50 | 
 51 |     double mag = sqrt(pow(person.velocity.x, 2) + pow(person.velocity.y, 2));
 52 |     double factor = 1.0 + mag * factor_;
 53 |     double point = get_radius(cutoff_, amplitude_, covar_ * factor);
 54 | 
 55 |     *min_x = std::min(*min_x, person.position.x - point);
 56 |     *min_y = std::min(*min_y, person.position.y - point);
 57 |     *max_x = std::max(*max_x, person.position.x + point);
 58 |     *max_y = std::max(*max_y, person.position.y + point);
 59 |   }
 60 | }
 61 | 
 62 | void ProxemicLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
 63 | {
 64 |   boost::recursive_mutex::scoped_lock lock(lock_);
 65 |   if (!enabled_) return;
 66 | 
 67 |   if (people_list_.people.size() == 0)
 68 |     return;
 69 |   if (cutoff_ >= amplitude_)
 70 |     return;
 71 | 
 72 |   std::list<people_msgs::Person>::iterator p_it;
 73 |   costmap_2d::Costmap2D* costmap = layered_costmap_->getCostmap();
 74 |   double res = costmap->getResolution();
 75 | 
 76 |   for (p_it = transformed_people_.begin(); p_it != transformed_people_.end(); ++p_it)
 77 |   {
 78 |     people_msgs::Person person = *p_it;
 79 |     double angle = atan2(person.velocity.y, person.velocity.x);
 80 |     double mag = sqrt(pow(person.velocity.x, 2) + pow(person.velocity.y, 2));
 81 |     double factor = 1.0 + mag * factor_;
 82 |     double base = get_radius(cutoff_, amplitude_, covar_);
 83 |     double point = get_radius(cutoff_, amplitude_, covar_ * factor);
 84 | 
 85 |     unsigned int width = std::max(1, static_cast<int>((base + point) / res)),
 86 |                  height = std::max(1, static_cast<int>((base + point) / res));
 87 | 
 88 |     double cx = person.position.x, cy = person.position.y;
 89 | 
 90 |     double ox, oy;
 91 |     if (sin(angle) > 0)
 92 |       oy = cy - base;
 93 |     else
 94 |       oy = cy + (point - base) * sin(angle) - base;
 95 | 
 96 |     if (cos(angle) >= 0)
 97 |       ox = cx - base;
 98 |     else
 99 |       ox = cx + (point - base) * cos(angle) - base;
100 | 
101 | 
102 |     int dx, dy;
103 |     costmap->worldToMapNoBounds(ox, oy, dx, dy);
104 | 
105 |     int start_x = 0, start_y = 0, end_x = width, end_y = height;
106 |     if (dx < 0)
107 |       start_x = -dx;
108 |     else if (dx + width > costmap->getSizeInCellsX())
109 |       end_x = std::max(0, static_cast<int>(costmap->getSizeInCellsX()) - dx);
110 | 
111 |     if (static_cast<int>(start_x + dx) < min_i)
112 |       start_x = min_i - dx;
113 |     if (static_cast<int>(end_x + dx) > max_i)
114 |       end_x = max_i - dx;
115 | 
116 |     if (dy < 0)
117 |       start_y = -dy;
118 |     else if (dy + height > costmap->getSizeInCellsY())
119 |       end_y = std::max(0, static_cast<int>(costmap->getSizeInCellsY()) - dy);
120 | 
121 |     if (static_cast<int>(start_y + dy) < min_j)
122 |       start_y = min_j - dy;
123 |     if (static_cast<int>(end_y + dy) > max_j)
124 |       end_y = max_j - dy;
125 | 
126 |     double bx = ox + res / 2,
127 |            by = oy + res / 2;
128 |     for (int i = start_x; i < end_x; i++)
129 |     {
130 |       for (int j = start_y; j < end_y; j++)
131 |       {
132 |         unsigned char old_cost = costmap->getCost(i + dx, j + dy);
133 |         if (old_cost == costmap_2d::NO_INFORMATION)
134 |           continue;
135 | 
136 |         double x = bx + i * res, y = by + j * res;
137 |         double ma = atan2(y - cy, x - cx);
138 |         double diff = angles::shortest_angular_distance(angle, ma);
139 |         double a;
140 |         if (fabs(diff) < M_PI / 2)
141 |           a = gaussian(x, y, cx, cy, amplitude_, covar_ * factor, covar_, angle);
142 |         else
143 |           a = gaussian(x, y, cx, cy, amplitude_, covar_,       covar_, 0);
144 | 
145 |         if (a < cutoff_)
146 |           continue;
147 |         unsigned char cvalue = (unsigned char) a;
148 |         costmap->setCost(i + dx, j + dy, std::max(cvalue, old_cost));
149 |       }
150 |     }
151 |   }
152 | }
153 | 
154 | void ProxemicLayer::configure(ProxemicLayerConfig &config, uint32_t level)
155 | {
156 |   cutoff_ = config.cutoff;
157 |   amplitude_ = config.amplitude;
158 |   covar_ = config.covariance;
159 |   factor_ = config.factor;
160 |   people_keep_time_ = ros::Duration(config.keep_time);
161 |   enabled_ = config.enabled;
162 | }
163 | };  // namespace social_navigation_layers
164 | 


--------------------------------------------------------------------------------
/social_navigation_layers/src/social_layer.cpp:
--------------------------------------------------------------------------------
  1 | // Copyright 2018 David V. Lu!!
  2 | #include <social_navigation_layers/social_layer.h>
  3 | #include <math.h>
  4 | #include <angles/angles.h>
  5 | #include <geometry_msgs/PointStamped.h>
  6 | #include <pluginlib/class_list_macros.h>
  7 | #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
  8 | #include <algorithm>
  9 | #include <string>
 10 | 
 11 | using costmap_2d::NO_INFORMATION;
 12 | using costmap_2d::LETHAL_OBSTACLE;
 13 | using costmap_2d::FREE_SPACE;
 14 | 
 15 | namespace social_navigation_layers
 16 | {
 17 | void SocialLayer::onInitialize()
 18 | {
 19 |   ros::NodeHandle nh("~/" + name_), g_nh;
 20 |   current_ = true;
 21 |   first_time_ = true;
 22 |   people_sub_ = nh.subscribe("/people", 1, &SocialLayer::peopleCallback, this);
 23 | }
 24 | 
 25 | void SocialLayer::peopleCallback(const people_msgs::People& people)
 26 | {
 27 |   boost::recursive_mutex::scoped_lock lock(lock_);
 28 |   people_list_ = people;
 29 | }
 30 | 
 31 | 
 32 | void SocialLayer::updateBounds(double origin_x, double origin_y, double origin_z, double* min_x, double* min_y,
 33 |                                double* max_x, double* max_y)
 34 | {
 35 |   boost::recursive_mutex::scoped_lock lock(lock_);
 36 | 
 37 |   std::string global_frame = layered_costmap_->getGlobalFrameID();
 38 |   transformed_people_.clear();
 39 | 
 40 |   for (unsigned int i = 0; i < people_list_.people.size(); i++)
 41 |   {
 42 |     people_msgs::Person& person = people_list_.people[i];
 43 |     people_msgs::Person tpt;
 44 |     geometry_msgs::PointStamped pt, opt;
 45 | 
 46 |     try
 47 |     {
 48 |       pt.point.x = person.position.x;
 49 |       pt.point.y = person.position.y;
 50 |       pt.point.z = person.position.z;
 51 |       pt.header.frame_id = people_list_.header.frame_id;
 52 |       pt.header.stamp = people_list_.header.stamp;
 53 |       tf_->transform(pt, opt, global_frame);
 54 |       tpt.position.x = opt.point.x;
 55 |       tpt.position.y = opt.point.y;
 56 |       tpt.position.z = opt.point.z;
 57 | 
 58 |       pt.point.x += person.velocity.x;
 59 |       pt.point.y += person.velocity.y;
 60 |       pt.point.z += person.velocity.z;
 61 |       tf_->transform(pt, opt, global_frame);
 62 | 
 63 |       tpt.velocity.x = opt.point.x - tpt.position.x;
 64 |       tpt.velocity.y = opt.point.y - tpt.position.y;
 65 |       tpt.velocity.z = opt.point.z - tpt.position.z;
 66 | 
 67 |       transformed_people_.push_back(tpt);
 68 |     }
 69 |     catch (tf2::LookupException& ex)
 70 |     {
 71 |       ROS_ERROR("No Transform available Error: %s\n", ex.what());
 72 |       continue;
 73 |     }
 74 |     catch (tf2::ConnectivityException& ex)
 75 |     {
 76 |       ROS_ERROR("Connectivity Error: %s\n", ex.what());
 77 |       continue;
 78 |     }
 79 |     catch (tf2::ExtrapolationException& ex)
 80 |     {
 81 |       ROS_ERROR("Extrapolation Error: %s\n", ex.what());
 82 |       continue;
 83 |     }
 84 |   }
 85 |   updateBoundsFromPeople(min_x, min_y, max_x, max_y);
 86 |   if (first_time_)
 87 |   {
 88 |     last_min_x_ = *min_x;
 89 |     last_min_y_ = *min_y;
 90 |     last_max_x_ = *max_x;
 91 |     last_max_y_ = *max_y;
 92 |     first_time_ = false;
 93 |   }
 94 |   else
 95 |   {
 96 |     double a = *min_x, b = *min_y, c = *max_x, d = *max_y;
 97 |     *min_x = std::min(last_min_x_, *min_x);
 98 |     *min_y = std::min(last_min_y_, *min_y);
 99 |     *max_x = std::max(last_max_x_, *max_x);
100 |     *max_y = std::max(last_max_y_, *max_y);
101 |     last_min_x_ = a;
102 |     last_min_y_ = b;
103 |     last_max_x_ = c;
104 |     last_max_y_ = d;
105 |   }
106 | }
107 | };  // namespace social_navigation_layers
108 | 


--------------------------------------------------------------------------------