From 792e844bd5b5c6a56087f42afb5cf9c288c40c1a Mon Sep 17 00:00:00 2001 From: atharvag Date: Thu, 8 Feb 2024 22:39:40 +0530 Subject: [PATCH] added brne inter planner --- planners/inter/brne_inter/CMakeLists.txt | 60 +++++ planners/inter/brne_inter/cfg/BrneInter.cfg | 17 ++ .../inter/brne_inter/include/brne_inter.h | 144 ++++++++++++ .../inter/brne_inter/mbf_inter_plugin.xml | 8 + planners/inter/brne_inter/package.xml | 68 ++++++ planners/inter/brne_inter/src/brne_inter.cpp | 217 ++++++++++++++++++ 6 files changed, 514 insertions(+) create mode 100644 planners/inter/brne_inter/CMakeLists.txt create mode 100644 planners/inter/brne_inter/cfg/BrneInter.cfg create mode 100644 planners/inter/brne_inter/include/brne_inter.h create mode 100644 planners/inter/brne_inter/mbf_inter_plugin.xml create mode 100644 planners/inter/brne_inter/package.xml create mode 100644 planners/inter/brne_inter/src/brne_inter.cpp diff --git a/planners/inter/brne_inter/CMakeLists.txt b/planners/inter/brne_inter/CMakeLists.txt new file mode 100644 index 0000000..1df8199 --- /dev/null +++ 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 ## +# ############ \ No newline at end of file diff --git a/planners/inter/brne_inter/cfg/BrneInter.cfg b/planners/inter/brne_inter/cfg/BrneInter.cfg new file mode 100644 index 0000000..c931ad7 --- /dev/null +++ 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")) \ No newline at end of file diff --git a/planners/inter/brne_inter/include/brne_inter.h b/planners/inter/brne_inter/include/brne_inter.h new file mode 100644 index 0000000..9c7039a --- /dev/null +++ b/planners/inter/brne_inter/include/brne_inter.h @@ -0,0 +1,144 @@ +#ifndef BRNE_INTER_H_ +#define BRNE_INTER_H_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include + + + +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 &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 &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 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 semanticPoints; + std::vector detectedRanges; + std::vector 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_ \ No newline at end of file diff --git a/planners/inter/brne_inter/mbf_inter_plugin.xml b/planners/inter/brne_inter/mbf_inter_plugin.xml new file mode 100644 index 0000000..ee32a02 --- /dev/null +++ b/planners/inter/brne_inter/mbf_inter_plugin.xml @@ -0,0 +1,8 @@ + + + + An intermediate planner that acts as an interface to the Bayes Rule Nash Equilibrium (BRNE) Social Navigation algorithm. + + + \ No newline at end of file diff --git a/planners/inter/brne_inter/package.xml b/planners/inter/brne_inter/package.xml new file mode 100644 index 0000000..d608a70 --- /dev/null +++ b/planners/inter/brne_inter/package.xml @@ -0,0 +1,68 @@ + + + brne_inter + 0.0.0 + The brne_inter package + + + + + Atharva Ghotavadekar + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + mbf_costmap_core + costmap_2d + inter_util + mbf_costmap_core + costmap_2d + inter_util + costmap_2d + mbf_costmap_core + inter_util + + + + + + + + \ No newline at end of file diff --git a/planners/inter/brne_inter/src/brne_inter.cpp b/planners/inter/brne_inter/src/brne_inter.cpp new file mode 100644 index 0000000..66806f6 --- /dev/null +++ b/planners/inter/brne_inter/src/brne_inter.cpp @@ -0,0 +1,217 @@ +#include "../include/brne_inter.h" +#include "../../inter_util/include/inter_util.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +PLUGINLIB_EXPORT_CLASS(brne_inter::BrneInter, mbf_costmap_core::CostmapInter) + +namespace brne_inter +{ + + uint32_t BrneInter::makePlan(const geometry_msgs::PoseStamped &start, const geometry_msgs::PoseStamped &goal, + std::vector &plan, double &cost, std::string &message) + { + ROS_INFO("Make plan called"); + boost::unique_lock plan_lock(plan_mtx_); + boost::unique_lock speed_lock(speed_mtx_); + + double robot_x = start.pose.position.x; + double robot_y = start.pose.position.y; + double robot_z = start.pose.position.z; + + plan.clear(); + plan = plan_; + + plan_lock.unlock(); + speed_lock.unlock(); + + return 0; + } + + bool BrneInter::setPlan(const std::vector &plan) + { + boost::unique_lock lock(plan_mtx_); + ROS_INFO("Set plan called"); + plan_ = plan; + lock.unlock(); + return true; + } + + void BrneInter::semanticCallback(const pedsim_msgs::SemanticData::ConstPtr &message) + // turns our semantic layer data into points we can use to calculate distance + { + boost::unique_lock lock(plan_mtx_); + + semanticPoints.clear(); + for (const auto &point : message->points) + { + geometry_msgs::Point32 pedestrianPoint; + pedestrianPoint.x = point.location.x; + pedestrianPoint.y = point.location.y; + pedestrianPoint.z = point.location.z; + semanticPoints.push_back(pedestrianPoint); + } + } + + void BrneInter::laserScanCallback(const sensor_msgs::LaserScan::ConstPtr &message) + { + boost::unique_lock lock(plan_mtx_); + + // Set a maximum distance threshold for wall detection (adjust as needed) + double max_detection_range = caution_detection_range_ + 0.5; // detect every obstacle in his caution_detection_range plus 0.5 metres + + detectedRanges.clear(); + // Accessing and printing range data + for (size_t i = 0; i < message->ranges.size(); ++i) + { + double angle = message->angle_min + i * message->angle_increment; + double range = message->ranges[i]; + + // Check if the range is under the maximum detection range + if (range < max_detection_range) + { + detectedRanges.push_back(range); + detectedAngles.push_back(angle); + } + } + + lock.unlock(); + } + + void BrneInter::initialize(std::string name, costmap_2d::Costmap2DROS *global_costmap_ros, costmap_2d::Costmap2DROS *local_costmap_ros) + { + this->name = name; + std::string node_namespace_ = ros::this_node::getNamespace(); + std::string semantic_layer = "/pedsim_agents/semantic/pedestrian"; + std::string optimal_path_topic = "/optimal_path"; + nh_ = ros::NodeHandle("~"); + subscriber_ = nh_.subscribe(semantic_layer, 1, &BrneInter::semanticCallback, this); + dangerPublisher = nh_.advertise("Danger", 10); + optimal_path_subscriber = nh_.subscribe(optimal_path_topic, 10, &BrneInter::optimalPathCallback, this); + + // testPublisher = nh_.advertise("TestPub", 10); + + + // Not seen in log. Maybe output is disabled? + ROS_INFO("Initializing inter planner: BRNE "); + + // get topic for our scan + std::string scan_topic_name; + std::string helios_points_topic_name; + if (!nh_.getParam(node_namespace_ + "/move_base_flex/local_costmap/obstacles_layer/scan/topic", scan_topic_name)) + { + ROS_ERROR("Failed to get parameter %s/move_base_flex/local_costmap/obstacles_layer/scan/topic", node_namespace_.c_str()); + if (!nh_.getParam(node_namespace_ + "/move_base_flex/local_costmap/obstacles_layer/helios_points/topic", helios_points_topic_name)) + { + ROS_ERROR("Failed to get parameter %s/move_base_flex/local_costmap/obstacles_layer/helios_points/topic", node_namespace_.c_str()); + } + } + if (!scan_topic_name.empty()) + { + laser_scan_subscriber_ = nh_.subscribe(scan_topic_name, 1, &BrneInter::laserScanCallback, this); + } + // if(!helios_points_topic_name.empty()){ + // helios_points_subscriber_ = nh_.subscribe(helios_points_topic_name, 1, &BrneInter::pointCloudCallback, this); + // } + + // get our local planner name + std::string planner_keyword; + if (!nh_.getParam(node_namespace_ + "/local_planner", planner_keyword)) + { + ROS_ERROR("Failed to get parameter %s/local_planner", node_namespace_.c_str()); + } + std::string local_planner_name = inter_util::InterUtil::getLocalPlanner(planner_keyword); + // get the starting parameter for max_vel_x from our planner + if (!nh_.getParam(node_namespace_ + "/move_base_flex/" + local_planner_name + "/max_vel_x", max_vel_x_param_)) + { + ROS_ERROR("Failed to get parameter %s/move_base_flex/%s/max_vel_x", node_namespace_.c_str(), local_planner_name.c_str()); + return; + } + // Create service client for the Reconfigure service + setParametersClient_ = nh_.serviceClient(node_namespace_ + "/move_base_flex/" + local_planner_name + "/set_parameters"); + dynamic_reconfigure::Server server; + server.setCallback(boost::bind(&BrneInter::reconfigure, this, _1, _2)); + + // thread to control the velocity for robot + velocity_thread_ = std::thread(&BrneInter::setMaxVelocityThread, this); + + // needs to be declared here because cautious_speed gets declared with reconfigure + changed_max_vel_x_param_ = (cautious_speed_ * max_vel_x_param_); + } + + void BrneInter::reconfigure(brne_inter::brneInterConfig &config, uint32_t level) + { + boost::unique_lock lock(plan_mtx_); + + // updating values from config + ped_minimum_distance_ = config.ped_minimum_distance; + temp_goal_distance_ = config.temp_goal_distance; + caution_detection_range_ = config.caution_detection_range; + cautious_speed_ = config.cautious_speed; + temp_goal_tolerance_ = config.temp_goal_tolerance; + fov_ = config.fov; + danger_threshold = config.danger_threshold; + changed_max_vel_x_param_ = (cautious_speed_ * max_vel_x_param_); + + lock.unlock(); + } + + void BrneInter::setMaxVelocityThread() + { + ros::Rate rate(1); // Adjust the rate as needed + while (ros::ok()) + { + // Lock to access shared variables + boost::unique_lock lock(speed_mtx_); + + // Check if the speed has changed + if (speed_ != last_speed_) + { + // set max_vel_x parameter + double_param_.name = "max_vel_x"; + double_param_.value = speed_; + conf_.doubles.clear(); + conf_.doubles.push_back(double_param_); + reconfig_.request.config = conf_; + + // Call setParametersClient_ to update parameters + if (setParametersClient_.call(reconfig_)) + { + ROS_INFO_ONCE("Dynamic reconfigure request successful"); + } + else + { + ROS_ERROR_ONCE("Failed to call dynamic reconfigure service"); + } + + // Update last_speed_ to avoid unnecessary calls + last_speed_ = speed_; + } + // Unlock and sleep + lock.unlock(); + rate.sleep(); + } + } + + void BrneInter::optimalPathCallback(const nav_msgs::Path::ConstPtr &msg){ + // Stores the optimal path received in plan_ + boost::unique_lock lock(plan_mtx_); + plan_ = msg->poses; + lock.unlock(); + + } + +} \ No newline at end of file