From dcaa435525b10fd4ccb08363a04cf6021bb8a474 Mon Sep 17 00:00:00 2001 From: omigeft Date: Tue, 17 Oct 2023 17:33:37 +0800 Subject: [PATCH] update: visualize apf --- .../apf_planner/include/apf_planner.h | 19 ++++-- .../apf_planner/src/apf_planner.cpp | 58 ++++++++++++++++++- .../config/costmap/global_costmap_params.yaml | 3 + .../config/costmap/local_costmap_params.yaml | 2 + .../config/planner/apf_planner_params.yaml | 10 +++- .../include/robots/start_robots.launch.xml | 4 +- src/sim_env/rviz/sim_env.rviz | 10 ++++ src/user_config/user_config.yaml | 4 +- 8 files changed, 97 insertions(+), 13 deletions(-) diff --git a/src/planner/local_planner/apf_planner/include/apf_planner.h b/src/planner/local_planner/apf_planner/include/apf_planner.h index 69b705c..2c2c4de 100644 --- a/src/planner/local_planner/apf_planner/include/apf_planner.h +++ b/src/planner/local_planner/apf_planner/include/apf_planner.h @@ -3,8 +3,8 @@ * @file: apf_planner.h * @breif: Contains the Artificial Potential Field (APF) local planner class * @author: Wu Maojia, Yang Haodong - * @update: 2023-10-1 - * @version: 1.0 + * @update: 2023-10-17 + * @version: 1.2 * * Copyright (c) 2023，Wu Maojia, Yang Haodong * All rights reserved. @@ -23,6 +23,7 @@ #include #include +#include #include #include @@ -110,11 +111,18 @@ class APFPlanner : public nav_core::BaseLocalPlanner, local_planner::LocalPlanne */ Eigen::Vector2d getRepulsiveForce(); + /** + * @brief Callback function of costmap_sub_ to publish /potential_map topic + * @param msg the message received from topic /move_base/local_costmap/costmap + */ + void publishPotentialMap(const nav_msgs::OccupancyGrid::ConstPtr& msg); + private: bool initialized_, goal_reached_; tf2_ros::Buffer* tf_; costmap_2d::Costmap2DROS* costmap_ros_; // costmap(ROS wrapper) unsigned char* local_costmap_; // costmap char map + nav_msgs::OccupancyGrid potential_map_; // local potential field map int plan_index_; std::vector global_plan_; @@ -131,12 +139,15 @@ class APFPlanner : public nav_core::BaseLocalPlanner, local_planner::LocalPlanne double zeta_, eta_; // scale factor of attractive and repulsive force - int cost_ub_, cost_lb_; // the upper and lower bound of costmap used to calculate repulsive force potential field + int cost_ub_, cost_lb_; // the upper and lower bound of costmap used to calculate potential field + + double inflation_radius_; // the costmap inflation radius of obstacles std::deque hist_nf_; // historical net forces base_local_planner::OdometryHelperRos* odom_helper_; - ros::Publisher target_pose_pub_, current_pose_pub_; + ros::Publisher target_pose_pub_, current_pose_pub_, potential_map_pub_; + ros::Subscriber costmap_sub_; // subscribe local map topic to generate potential field double goal_x_, goal_y_; Eigen::Vector3d goal_rpy_; diff --git a/src/planner/local_planner/apf_planner/src/apf_planner.cpp b/src/planner/local_planner/apf_planner/src/apf_planner.cpp index 46659f2..8d2066e 100644 --- a/src/planner/local_planner/apf_planner/src/apf_planner.cpp +++ b/src/planner/local_planner/apf_planner/src/apf_planner.cpp @@ -3,8 +3,8 @@ * @file: apf_planner.cpp * @breif: Contains the Artificial Potential Field (APF) local planner class * @author: Wu Maojia, Yang Haodong - * @update: 2023-10-2 - * @version: 1.1 + * @update: 2023-10-17 + * @version: 1.2 * * Copyright (c) 2023，Wu Maojia, Yang Haodong * All rights reserved. @@ -88,6 +88,8 @@ void APFPlanner::initialize(std::string name, tf2_ros::Buffer* tf, costmap_2d::C nh.param("cost_ub", cost_ub_, (int)lethal_cost_); nh.param("cost_lb", cost_lb_, 0); + nh.param("inflation_radius", inflation_radius_, 1.0); + nh.param("base_frame", base_frame_, base_frame_); nh.param("map_frame", map_frame_, map_frame_); @@ -100,6 +102,9 @@ void APFPlanner::initialize(std::string name, tf2_ros::Buffer* tf, costmap_2d::C odom_helper_ = new base_local_planner::OdometryHelperRos("/odom"); target_pose_pub_ = nh.advertise("/target_pose", 10); current_pose_pub_ = nh.advertise("/current_pose", 10); + potential_map_pub_ = nh.advertise("/potential_map", 10); + costmap_sub_ = nh.subscribe( + "/move_base/local_costmap/costmap", 10, &APFPlanner::publishPotentialMap, this); ROS_INFO("APF planner initialized!"); } @@ -383,4 +388,53 @@ Eigen::Vector2d APFPlanner::getRepulsiveForce() return rep_force; } + +/** + * @brief Callback function of costmap_sub_ to publish /potential_map topic + * @param msg the message received from topic /move_base/local_costmap/costmap + */ +void APFPlanner::publishPotentialMap(const nav_msgs::OccupancyGrid::ConstPtr& msg) +{ + int nx = potential_map_.info.width, ny = potential_map_.info.height; + double attr_scale = zeta_ / (zeta_ + eta_), rep_scale = eta_ / (zeta_ + eta_); // the two potential scales sum up to 1 + double current_cost, dist_to_target, dist_to_obstacles, inflation_radius, scaled_attr, scaled_rep; + double bound_diff = cost_ub_ - cost_lb_; + int tx, ty; // costmap coordinates of target point + + // calculate costmap coordinates from world coordinates (maybe out of bound) + tx = (int)((target_ps_.pose.position.x - current_ps_.pose.position.x - origin_x_) / resolution_ - convert_offset_); + ty = (int)((target_ps_.pose.position.y - current_ps_.pose.position.y - origin_y_) / resolution_ - convert_offset_); + + // calculate distance from the robot to target (on the scale of costmap) + dist_to_target = std::hypot( + tx - (int)((- origin_x_) / resolution_ - convert_offset_), + ty - (int)((- origin_y_) / resolution_ - convert_offset_) + ); + + potential_map_ = *msg; + for (int y = 0; y < ny; ++y) + { + for (int x = 0; x < nx; ++x) + { + // temp variables + current_cost = potential_map_.data[x + nx * y]; // cost of the cell + dist_to_obstacles = (cost_ub_ - current_cost) / bound_diff; // distance from cell to obstacles + // (normalized scale, i.e., within the range of [0,1]) + inflation_radius = inflation_radius_ / resolution_; // the costmap inflation radius + // of obstacles (on the scale of costmap) + + // to calculate the two scaled force potential fields + scaled_attr = attr_scale * + ((std::hypot(tx - x, ty - y) - dist_to_target) / inflation_radius / 2.0 + 0.5); + scaled_rep = rep_scale * std::pow(1.0 / dist_to_obstacles - 1.0, 2); + + // sum two potential fields to calculate the net potential field + potential_map_.data[x + nx * y] = + std::max(cost_lb_, std::min(cost_ub_ - 1, (int)((scaled_attr + scaled_rep) * bound_diff))); + } + } + + // publish potential map + potential_map_pub_.publish(potential_map_); +} } // namespace apf_planner \ No newline at end of file diff --git a/src/sim_env/config/costmap/global_costmap_params.yaml b/src/sim_env/config/costmap/global_costmap_params.yaml index 61918d3..2c07e51 100644 --- a/src/sim_env/config/costmap/global_costmap_params.yaml +++ b/src/sim_env/config/costmap/global_costmap_params.yaml @@ -7,6 +7,9 @@ global_costmap: transform_tolerance: 0.5 static_map: true + + inflation_radius: 1.0 + # plugins: # - {name: static_map, type: "costmap_2d::StaticLayer"} # # - {name: voxel_layer, type: "costmap_2d::VoxelLayer"} diff --git a/src/sim_env/config/costmap/local_costmap_params.yaml b/src/sim_env/config/costmap/local_costmap_params.yaml index 2e900aa..8d8beed 100644 --- a/src/sim_env/config/costmap/local_costmap_params.yaml +++ b/src/sim_env/config/costmap/local_costmap_params.yaml @@ -11,6 +11,8 @@ local_costmap: width: 3 height: 3 resolution: 0.05 + + inflation_radius: 1.0 # plugins: # - {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"} diff --git a/src/sim_env/config/planner/apf_planner_params.yaml b/src/sim_env/config/planner/apf_planner_params.yaml index 9f398f5..0286d09 100644 --- a/src/sim_env/config/planner/apf_planner_params.yaml +++ b/src/sim_env/config/planner/apf_planner_params.yaml @@ -21,11 +21,15 @@ APFPlanner: s_window: 5 # trajectory smoothing window time - zeta: 3.0 # scale factor of attractive force + zeta: 1.0 # scale factor of attractive force eta: 1.0 # scale factor of repulsive force - cost_ub: 253 # the upper bound of costmap used to calculate repulsive force potential field - cost_lb: 0 # the lower bound of costmap used to calculate repulsive force potential field + cost_ub: 101 # the upper bound of costmap used to calculate potential field + cost_lb: 0 # the lower bound of costmap used to calculate potential field + + # the costmap inflation radius of obstacles + # this parameter should be consistent with that in sim_env/config/costmap/xxx_costmap_params.yaml + inflation_radius: 1.0 base_frame: base_link map_frame: map \ No newline at end of file diff --git a/src/sim_env/launch/include/robots/start_robots.launch.xml b/src/sim_env/launch/include/robots/start_robots.launch.xml index 21c824a..1b8a4b8 100644 --- a/src/sim_env/launch/include/robots/start_robots.launch.xml +++ b/src/sim_env/launch/include/robots/start_robots.launch.xml @@ -2,8 +2,8 @@ - - + + diff --git a/src/sim_env/rviz/sim_env.rviz b/src/sim_env/rviz/sim_env.rviz index c268f66..76feb62 100644 --- a/src/sim_env/rviz/sim_env.rviz +++ b/src/sim_env/rviz/sim_env.rviz @@ -346,6 +346,16 @@ Visualization Manager: Unreliable: false Use Timestamp: false Value: true + - Alpha: 0.20000000298023224 + Class: rviz/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: Map + Topic: /potential_map + Unreliable: false + Use Timestamp: false + Value: false Enabled: true Global Options: Background Color: 48; 48; 48 diff --git a/src/user_config/user_config.yaml b/src/user_config/user_config.yaml index 66fff96..ed3629b 100644 --- a/src/user_config/user_config.yaml +++ b/src/user_config/user_config.yaml @@ -4,8 +4,8 @@ rviz_file: "sim_env.rviz" robots_config: - robot1_type: "turtlebot3_waffle" - robot1_global_planner: "a_star" - robot1_local_planner: "pid" + robot1_global_planner: "theta_star" + robot1_local_planner: "apf" robot1_x_pos: "0.0" robot1_y_pos: "0.0" robot1_z_pos: "0.0"