update: visualize apf

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 <geometry_msgs/Twist.h>
 
 #include <nav_msgs/Odometry.h>
+#include <nav_msgs/OccupancyGrid.h>
 #include <base_local_planner/odometry_helper_ros.h>
 #include <tf2/utils.h>
 
@@ -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<geometry_msgs::PoseStamped> 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<Eigen::Vector2d> 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_;

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<geometry_msgs::PoseStamped>("/target_pose", 10);
     current_pose_pub_ = nh.advertise<geometry_msgs::PoseStamped>("/current_pose", 10);
+    potential_map_pub_ = nh.advertise<nav_msgs::OccupancyGrid>("/potential_map", 10);
+    costmap_sub_ = nh.subscribe<nav_msgs::OccupancyGrid>(
+        "/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

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"}

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"}

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

src/sim_env/launch/include/robots/start_robots.launch.xml

@@ -2,8 +2,8 @@
 <launch>
 	<arg name="robot_number" default="1" />
 	<arg name="robot1_type" value="turtlebot3_waffle" />
-	<arg name="robot1_global_planner" value="a_star" />
-	<arg name="robot1_local_planner" value="pid" />
+	<arg name="robot1_global_planner" value="theta_star" />
+	<arg name="robot1_local_planner" value="apf" />
 	<arg name="robot1_x_pos" value="0.0" />
 	<arg name="robot1_y_pos" value="0.0" />
 	<arg name="robot1_z_pos" value="0.0" />

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

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"