├── .gitignore ├── navigation_layers ├── CMakeLists.txt ├── CHANGELOG.rst └── package.xml ├── range_sensor_layer ├── README.md ├── costmap_plugins.xml ├── scripts │ └── send_message.py ├── cfg │ └── RangeSensorLayer.cfg ├── package.xml ├── CHANGELOG.rst ├── CMakeLists.txt ├── include │ └── range_sensor_layer │ │ └── range_sensor_layer.h └── src │ └── range_sensor_layer.cpp └── social_navigation_layers ├── costmap_plugins.xml ├── CHANGELOG.rst ├── cfg └── ProxemicLayer.cfg ├── package.xml ├── include └── social_navigation_layers │ ├── proxemic_layer.h │ └── social_layer.h ├── CMakeLists.txt └── src ├── social_layer.cpp ├── proxemic_layer.cpp └── passing_layer.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | build/ 2 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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/costmap_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | A range-sensor (sonar, IR) based obstacle layer for costmap_2d 4 | 5 | 6 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /social_navigation_layers/costmap_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | Uses people information to change the costmap 4 | 5 | 6 | Uses people information to change the costmap 7 | 8 | 9 | -------------------------------------------------------------------------------- /navigation_layers/package.xml: -------------------------------------------------------------------------------- 1 | 2 | navigation_layers 3 | 0.5.0 4 | Extra navigation layers. 5 | David V. Lu!! 6 | David V. Lu!! 7 | BSD 8 | 9 | catkin 10 | range_sensor_layer 11 | social_navigation_layers 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /range_sensor_layer/package.xml: -------------------------------------------------------------------------------- 1 | 2 | range_sensor_layer 3 | 0.5.0 4 | 5 | Navigation Layer for Range sensors like sonar and IR 6 | 7 | 8 | David V. Lu!! 9 | David!! 10 | 11 | BSD 12 | 13 | http://wiki.ros.org/range_sensor_layer 14 | 15 | catkin 16 | angles 17 | costmap_2d 18 | dynamic_reconfigure 19 | geometry_msgs 20 | pluginlib 21 | roscpp 22 | rospy 23 | sensor_msgs 24 | tf2_geometry_msgs 25 | roslint 26 | 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /social_navigation_layers/package.xml: -------------------------------------------------------------------------------- 1 | 2 | social_navigation_layers 3 | 0.5.0 4 | 5 | Plugin-based layers for the navigation stack that 6 | implement various social navigation contraints, like proxemic distance. 7 | 8 | David V. Lu!! 9 | David V. Lu!! 10 | BSD 11 | http://ros.org/wiki/social_navigation_layers 12 | 13 | catkin 14 | angles 15 | costmap_2d 16 | dynamic_reconfigure 17 | geometry_msgs 18 | people_msgs 19 | pluginlib 20 | roscpp 21 | roslint 22 | 23 | 24 | 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /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 `_) 9 | Any value set for max_angle gets overwritten in updateCostmap() so the 10 | parameter can safely be removed. 11 | * Issue `#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 `_) 12 | * Add buffer clearing when calling deactivate() or activate(). (`#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 | -------------------------------------------------------------------------------- /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 5 | #include 6 | #include 7 | #include 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* server_; 30 | dynamic_reconfigure::Server::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 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 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 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/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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /social_navigation_layers/src/social_layer.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2018 David V. Lu!! 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 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 | -------------------------------------------------------------------------------- /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 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 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(c) / costmap_2d::LETHAL_OBSTACLE; 63 | } 64 | unsigned char to_cost(double p) 65 | { 66 | return static_cast(p * costmap_2d::LETHAL_OBSTACLE); 67 | } 68 | 69 | boost::function processRangeMessageFunc_; 70 | boost::mutex range_message_mutex_; 71 | std::list range_msgs_buffer_; 72 | std::map, 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 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 *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 | -------------------------------------------------------------------------------- /social_navigation_layers/src/proxemic_layer.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2018 David V. Lu!! 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 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(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::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::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((base + point) / res)), 86 | height = std::max(1, static_cast((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(costmap->getSizeInCellsX()) - dx); 110 | 111 | if (static_cast(start_x + dx) < min_i) 112 | start_x = min_i - dx; 113 | if (static_cast(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(costmap->getSizeInCellsY()) - dy); 120 | 121 | if (static_cast(start_y + dy) < min_j) 122 | start_y = min_j - dy; 123 | if (static_cast(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/passing_layer.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2018 David V. Lu!! 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 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::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((base + point) / res)), 108 | height = std::max(1, static_cast((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(costmap->getSizeInCellsX()) - dx); 132 | 133 | if (static_cast(start_x + dx) < min_i) 134 | start_x = min_i - dx; 135 | if (static_cast(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(costmap->getSizeInCellsY()) - dy); 142 | 143 | if (static_cast(start_y + dy) < min_j) 144 | start_y = min_j - dy; 145 | if (static_cast(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 | -------------------------------------------------------------------------------- /range_sensor_layer/src/range_sensor_layer.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2018 David V. Lu!! 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 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 = "/sonar"; 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(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(nh); 113 | dynamic_reconfigure::Server::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 range_msgs_buffer_copy; 190 | 191 | range_message_mutex_.lock(); 192 | range_msgs_buffer_copy = std::list(range_msgs_buffer_); 193 | range_msgs_buffer_.clear(); 194 | range_message_mutex_.unlock(); 195 | 196 | for (std::list::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(size_x_), bx1); 327 | by1 = std::min(static_cast(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, 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 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, 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::max(); 426 | max_x_ = max_y_ = -std::numeric_limits::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 | --------------------------------------------------------------------------------