Arena-Rosnav · Atharva-05 · Feb 8, 2024
diff --git a/planners/inter/brne_inter/CMakeLists.txt b/planners/inter/brne_inter/CMakeLists.txt
@@ -0,0 +1,60 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(brne_inter)
+
+find_package(catkin REQUIRED COMPONENTS
+  mbf_costmap_core
+  inter_util
+)
+
+generate_dynamic_reconfigure_options(
+  cfg/BrneInter.cfg
+)
+
+catkin_package(
+  INCLUDE_DIRS include
+  LIBRARIES brne_inter
+  CATKIN_DEPENDS mbf_costmap_core inter_util
+)
+
+# ##########
+# # Build ##
+# ##########
+include_directories(
+
+  # include
+  ${catkin_INCLUDE_DIRS}
+)
+
+add_library(brne_inter src/brne_inter.cpp)
+
+add_dependencies(brne_inter ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+add_dependencies(brne_inter ${PROJECT_NAME}_gencfg)
+
+target_link_libraries(brne_inter
+  ${catkin_LIBRARIES}
+)
+
+# ############
+# # Install ##
+# ############
+install(TARGETS brne_inter
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+)
+
+install(DIRECTORY include/${PROJECT_NAME}/
+  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+)
+
+install(FILES mbf_inter_plugin.xml
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+
+install(DIRECTORY include/${PROJECT_NAME}/
+  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+)
+
+# ############
+# # Testing ##
+# ############
diff --git a/planners/inter/brne_inter/cfg/BrneInter.cfg b/planners/inter/brne_inter/cfg/BrneInter.cfg
@@ -0,0 +1,17 @@
+#!/usr/bin/env python
+
+PACKAGE = 'brne_inter'
+
+from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, str_t, double_t, bool_t, int_t
+import math
+
+gen = ParameterGenerator()
+
+gen.add("caution_detection_range", double_t, 0, "range for detecting pedestrians to slow robot down", 4.0, 0.0, 50.0)
+gen.add("cautious_speed", double_t, 0, "speed multiplicator when detecting pedestrians in robot range", 0.5, 0.01, 1.0)
+gen.add("ped_minimum_distance", double_t, 0, "minimum distance to pedestrians", 2.0, 0.0, 10.0)
+gen.add("temp_goal_distance", double_t, 0, "distance for temporary goal behind robot to evade pedestrian", 0.7, 0.0, 10.0)
+gen.add("temp_goal_tolerance", double_t, 0, "tolerance how many metres the robot can be away from the temp goal until normal planning starts again", 0.25, 0.0, 2.0)
+gen.add("danger_threshold", double_t, 0, "threshold at which warning signal is being published", 0.6, 0.0, 1.0)
+gen.add("fov", double_t, 0, "", math.pi, 0.0, math.pi*2)
+exit(gen.generate(PACKAGE, "brne_inter", "brneInter"))
diff --git a/planners/inter/brne_inter/include/brne_inter.h b/planners/inter/brne_inter/include/brne_inter.h
@@ -0,0 +1,144 @@
+#ifndef BRNE_INTER_H_
+#define BRNE_INTER_H_
+
+#include <ros/ros.h>
+#include <mbf_costmap_core/costmap_inter.h>
+#include <boost/thread/mutex.hpp>
+#include <dynamic_reconfigure/server.h>
+#include <laser_geometry/laser_geometry.h>
+#include <sensor_msgs/PointCloud2.h>
+#include <sensor_msgs/PointCloud.h>
+#include <sensor_msgs/LaserScan.h>
+#include <sensor_msgs/point_cloud_conversion.h>
+#include <brne_inter/brneInterConfig.h>
+
+#include <std_msgs/Float64.h>
+#include <thread>
+
+#include <nav_msgs/Path.h>
+
+
+
+namespace brne_inter
+{
+
+    class BrneInter : public mbf_costmap_core::CostmapInter
+    {
+
+        using mbf_costmap_core::CostmapInter::CostmapInter;
+
+        /**
+         * @brief Given a goal pose in the world, compute a plan
+         * @param start The start pose
+         * @param goal The goal pose
+         * @param plan The plan... filled by the inter
+         * @param cost The cost for the the plan
+         * @param message Optional more detailed outcome as a string
+         * @return Result code as described on GetInterPath action result:
+         *         SUCCESS         = 0
+         *         1..9 are reserved as plugin specific non-error results
+         *         FAILURE         = 50  # Unspecified failure, only used for old, non-mfb_core based plugins
+         *         CANCELED        = 51
+         *         INVALID_START   = 52
+         *         INVALID_GOAL    = 53
+         *         BLOCKED_START   = 54
+         *         BLOCKED_GOAL    = 55
+         *         NO_PATH_FOUND   = 56
+         *         PAT_EXCEEDED    = 57
+         *         EMPTY_PATH      = 58
+         *         TF_ERROR        = 59
+         *         NOT_INITIALIZED = 60
+         *         INVALID_PLUGIN  = 61
+         *         INTERNAL_ERROR  = 62
+         *         71..99 are reserved as plugin specific errors
+         */
+        uint32_t makePlan(const geometry_msgs::PoseStamped &start, const geometry_msgs::PoseStamped &goal,
+                          std::vector<geometry_msgs::PoseStamped> &plan, double &cost, std::string &message);
+
+        /**
+         * @brief  Set the plan that the planner is following
+         * @param plan The plan to pass to the inter
+         * @return True if the plan was updated successfully, false otherwise
+         */
+        bool setPlan(const std::vector<geometry_msgs::PoseStamped> &plan);
+
+        /**
+         * @brief Requests the inter to cancel, e.g. if it takes too much time.
+         * @remark New on MBF API
+         * @return True if a cancel has been successfully requested, false if not implemented.
+         */
+        bool cancel() { return false; };
+
+        /**
+         * @brief Initialization function for the CostmapInter
+         * @param name The name of this inter
+         * @param costmap_ros A pointer to the ROS wrapper of the costmap to use for planning
+         */
+        void initialize(std::string name, costmap_2d::Costmap2DROS *global_costmap_ros, costmap_2d::Costmap2DROS *local_costmap_ros);
+
+
+    private:
+        // mutexes
+        boost::mutex plan_mtx_;
+        boost::mutex speed_mtx_;
+
+        // storage for setPlan
+        std::vector<geometry_msgs::PoseStamped> plan_;
+
+        // could be used for nh
+        std::string name;
+        std::string node_namespace_;
+
+        ros::Time start_timer_;
+        ros::NodeHandle nh_;
+
+        // default values
+        // change in BrneInter.cfg to your preference
+        double caution_detection_range_ = 10.0;
+        double cautious_speed_ = 0.1;
+        double ped_minimum_distance_ = 2.0;
+        double temp_goal_distance_ = 2.0;
+        double temp_goal_tolerance_ = 0.2;
+        double fov_ = M_PI;
+        double danger_threshold = 0.6;
+
+
+        // variables to control the speed
+        double speed_;
+        double last_speed_;
+        std::thread velocity_thread_;
+
+        ros::Subscriber subscriber_;
+        ros::Subscriber laser_scan_subscriber_;
+        ros::Subscriber helios_points_subscriber_;
+        ros::Subscriber optimal_path_subscriber;
+
+        ros::Publisher dangerPublisher;
+        ros::Publisher pathMarkerPublisher;
+
+        ros::ServiceClient setParametersClient_;
+
+        geometry_msgs::PoseStamped temp_goal_;
+        bool new_goal_set_ = false;
+
+        double max_vel_x_param_;
+        double changed_max_vel_x_param_;
+
+        dynamic_reconfigure::Reconfigure reconfig_;
+        dynamic_reconfigure::DoubleParameter double_param_;
+        dynamic_reconfigure::Config conf_;
+        std::vector<geometry_msgs::Point32> semanticPoints;
+        std::vector<double> detectedRanges;
+        std::vector<double> detectedAngles;
+
+        void reconfigure(brne_inter::brneInterConfig &config, uint32_t level);
+        void semanticCallback(const pedsim_msgs::SemanticData::ConstPtr& message);
+        void laserScanCallback(const sensor_msgs::LaserScan::ConstPtr& msg);
+        //void pointCloudCallback(const sensor_msgs::PointCloud2::ConstPtr& msg);
+        void setMaxVelocityThread();
+
+        void optimalPathCallback(const nav_msgs::Path::ConstPtr& msg);
+    };
+}
+
+#endif // SIDEWAYS_INTER_H_
diff --git a/planners/inter/brne_inter/mbf_inter_plugin.xml b/planners/inter/brne_inter/mbf_inter_plugin.xml
@@ -0,0 +1,8 @@
+<library path="lib/libbrne_inter">
+    <class name="brne_inter/BrneInter" type="brne_inter::BrneInter"
+        base_class_type="mbf_costmap_core::CostmapInter">
+        <description>
+            An intermediate planner that acts as an interface to the Bayes Rule Nash Equilibrium (BRNE) Social Navigation algorithm.
+        </description>
+    </class>
+</library>
diff --git a/planners/inter/brne_inter/package.xml b/planners/inter/brne_inter/package.xml
@@ -0,0 +1,68 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>brne_inter</name>
+  <version>0.0.0</version>
+  <description>The brne_inter package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="atharvagh1@gmail.com">Atharva Ghotavadekar</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/sideways_inter</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>mbf_costmap_core</build_depend>
+  <build_depend>costmap_2d</build_depend>
+  <build_depend>inter_util</build_depend> 
+  <build_export_depend>mbf_costmap_core</build_export_depend>
+  <build_export_depend>costmap_2d</build_export_depend>
+  <build_export_depend>inter_util</build_export_depend>
+  <exec_depend>costmap_2d</exec_depend>
+  <exec_depend>mbf_costmap_core</exec_depend>
+  <exec_depend>inter_util</exec_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+    <mbf_costmap_core plugin="${prefix}/mbf_inter_plugin.xml" />
+  </export>
+</package>