review multi agent and global_planner->graph_planner->a_star

src/core/global_planner/global_planner/include/global_planner.h

@@ -37,11 +37,7 @@ class GlobalPlanner
    * @brief Construct a new Global Planner object
    * @param costmap   the environment for path planning
    */
-  GlobalPlanner(costmap_2d::Costmap2D* costmap) : factor_(0.5f), map_size_{ 0 }
-  {
-    costmap_ = costmap;
-    map_size_ = static_cast<int>(costmap->getSizeInCellsX() * costmap->getSizeInCellsY());
-  }
+  GlobalPlanner(costmap_2d::Costmap2D* costmap);
 
   /**
    * @brief Destroy the Global Planner object

src/core/global_planner/global_planner/src/global_planner.cpp

@@ -18,6 +18,16 @@
 
 namespace global_planner
 {
+/**
+ * @brief Construct a new Global Planner object
+ * @param costmap   the environment for path planning
+ */
+GlobalPlanner::GlobalPlanner(costmap_2d::Costmap2D* costmap) : factor_(0.5f), map_size_{ 0 }
+{
+  costmap_ = costmap;
+  map_size_ = static_cast<int>(costmap->getSizeInCellsX() * costmap->getSizeInCellsY());
+}
+
 /**
  * @brief Set or reset obstacle factor
  * @param factor obstacle factor
@@ -101,14 +111,18 @@ void GlobalPlanner::outlineMap()
   auto nx = costmap_->getSizeInCellsX();
   auto ny = costmap_->getSizeInCellsY();
   auto pc = costmap_->getCharMap();
+
   for (int i = 0; i < nx; i++)
     *pc++ = costmap_2d::LETHAL_OBSTACLE;
+
   pc = costmap_->getCharMap() + (ny - 1) * nx;
   for (int i = 0; i < nx; i++)
     *pc++ = costmap_2d::LETHAL_OBSTACLE;
+
   pc = costmap_->getCharMap();
   for (int i = 0; i < ny; i++, pc += nx)
     *pc = costmap_2d::LETHAL_OBSTACLE;
+
   pc = costmap_->getCharMap() + nx - 1;
   for (int i = 0; i < ny; i++, pc += nx)
     *pc = costmap_2d::LETHAL_OBSTACLE;
@@ -125,17 +139,21 @@ std::vector<Node> GlobalPlanner::_convertClosedListToPath(std::unordered_map<int
                                                           const Node& goal)
 {
   std::vector<Node> path;
+
   auto current = closed_list.find(goal.id());
   while (current->second != start)
   {
     path.emplace_back(current->second.x(), current->second.y());
+
     auto it = closed_list.find(current->second.pid());
     if (it != closed_list.end())
       current = it;
     else
       return {};
   }
+
   path.push_back(start);
+
   return path;
 }
 

src/core/global_planner/graph_planner/src/a_star.cpp

@@ -30,7 +30,7 @@ namespace global_planner
  */
 AStar::AStar(costmap_2d::Costmap2D* costmap, bool dijkstra, bool gbfs) : GlobalPlanner(costmap)
 {
-  // can not using both dijkstra and GBFS at the same time
+  // can not use both dijkstra and GBFS at the same time
   if (!(dijkstra && gbfs))
   {
     is_dijkstra_ = dijkstra;
@@ -70,11 +70,11 @@ bool AStar::plan(const Node& start, const Node& goal, std::vector<Node>& path, s
   while (!open_list.empty())
   {
     // pop current node from open list
-    Node current = open_list.top();
+    auto current = open_list.top();
     open_list.pop();
 
-    // current node does not exist in closed list
-    if (closed_list.find(current.id()) != closed_list.end())
+    // current node exist in closed list, continue
+    if (closed_list.count(current.id()))
       continue;
 
     closed_list.insert(std::make_pair(current.id(), current));
@@ -91,15 +91,14 @@ bool AStar::plan(const Node& start, const Node& goal, std::vector<Node>& path, s
     for (const auto& motion : motions)
     {
       // explore a new node
-      Node node_new = current + motion;
+      auto node_new = current + motion;  // including current.g + motion.g
       node_new.set_id(grid2Index(node_new.x(), node_new.y()));
+      node_new.set_pid(current.id());
 
-      // node_new in closed list
-      if (closed_list.find(node_new.id()) != closed_list.end())
+      // node_new in closed list, continue
+      if (closed_list.count(node_new.id()))
         continue;
 
-      node_new.set_pid(current.id());
-
       // next node hit the boundary or obstacle
       // prevent planning failed when the current within inflation
       if ((node_new.id() < 0) || (node_new.id() >= map_size_) ||

src/core/global_planner/graph_planner/src/graph_planner.cpp

@@ -79,15 +79,17 @@ void GraphPlanner::initialize(std::string name)
     // costmap frame ID
     frame_id_ = costmap_ros_->getGlobalFrameID();
 
+    private_nh.param("obstacle_factor", factor_, 0.5);        // obstacle factor
     private_nh.param("default_tolerance", tolerance_, 0.0);   // error tolerance
     private_nh.param("outline_map", is_outline_, false);      // whether outline the map or not
-    private_nh.param("obstacle_factor", factor_, 0.5);        // obstacle factor, NOTE: no use...
     private_nh.param("expand_zone", is_expand_, false);       // whether publish expand zone or not
     private_nh.param("voronoi_map", is_voronoi_map_, false);  // whether to store Voronoi map or not
 
     // planner name
-    private_nh.param("planner_name", planner_name_, (std::string) "a_star");
+    private_nh.param("planner_name", planner_name_, std::string("a_star"));
+
     auto costmap = costmap_ros_->getCostmap();
+
     if (planner_name_ == "a_star")
       g_planner_ = std::make_shared<global_planner::AStar>(costmap);
     else if (planner_name_ == "dijkstra")
@@ -122,7 +124,6 @@ void GraphPlanner::initialize(std::string name)
     else
       ROS_ERROR("Unknown planner name: %s", planner_name_.c_str());
 
-    // pass costmap information to planner (required)
     g_planner_->setFactor(factor_);
 
     ROS_INFO("Using global graph planner: %s", planner_name_.c_str());
@@ -173,34 +174,31 @@ bool GraphPlanner::makePlan(const geometry_msgs::PoseStamped& start, const geome
     ROS_ERROR("This planner has not been initialized yet, but it is being used, please call initialize() before use");
     return false;
   }
+
   // clear existing plan
   plan.clear();
 
-  // judege whether goal and start node in costmap frame or not
-  if (goal.header.frame_id != frame_id_)
-  {
-    ROS_ERROR("The goal pose passed to this planner must be in the %s frame. It is instead in the %s frame.",
-              frame_id_.c_str(), goal.header.frame_id.c_str());
-    return false;
-  }
-
-  if (start.header.frame_id != frame_id_)
+  // judege whether start and goal node in costmap frame or not
+  if (start.header.frame_id != frame_id_ || goal.header.frame_id != frame_id_)
   {
-    ROS_ERROR("The start pose passed to this planner must be in the %s frame. It is instead in the %s frame.",
-              frame_id_.c_str(), start.header.frame_id.c_str());
+    ROS_ERROR("The start or goal pose passed to this planner must be in %s frame. It is instead in %s and %s frame.",
+              frame_id_.c_str(), start.header.frame_id.c_str(), goal.header.frame_id.c_str());
     return false;
   }
 
   // get goal and start node coordinate tranform from world to costmap
-  double wx = start.pose.position.x, wy = start.pose.position.y;
   unsigned int g_start_x, g_start_y, g_goal_x, g_goal_y;
+  double wx, wy;
+
+  wx = start.pose.position.x, wy = start.pose.position.y;
   if (!g_planner_->world2Map(wx, wy, g_start_x, g_start_y))
   {
     ROS_WARN(
         "The robot's start position is off the global costmap. Planning will always fail, are you sure the robot has "
         "been properly localized?");
     return false;
   }
+
   wx = goal.pose.position.x, wy = goal.pose.position.y;
   if (!g_planner_->world2Map(wx, wy, g_goal_x, g_goal_y))
   {
@@ -210,10 +208,6 @@ bool GraphPlanner::makePlan(const geometry_msgs::PoseStamped& start, const geome
     return false;
   }
 
-  // NOTE: how to init start and goal?
-  Node start_node(g_start_x, g_start_y, 0, 0, g_planner_->grid2Index(g_start_x, g_start_y), 0);
-  Node goal_node(g_goal_x, g_goal_y, 0, 0, g_planner_->grid2Index(g_goal_x, g_goal_y), 0);
-
   // outline the map
   if (is_outline_)
     g_planner_->outlineMap();
@@ -244,6 +238,10 @@ bool GraphPlanner::makePlan(const geometry_msgs::PoseStamped& start, const geome
   std::vector<Node> expand;
   bool path_found = false;
 
+  // init start and goal
+  Node start_node(g_start_x, g_start_y, 0, 0, g_planner_->grid2Index(g_start_x, g_start_y), -1);
+  Node goal_node(g_goal_x, g_goal_y, 0, 0, g_planner_->grid2Index(g_goal_x, g_goal_y), -1);
+
   // planning
   if (planner_name_ == "voronoi")
   {
@@ -307,7 +305,7 @@ void GraphPlanner::publishPlan(const std::vector<geometry_msgs::PoseStamped>& pl
   gui_plan.poses.resize(plan.size());
   gui_plan.header.frame_id = frame_id_;
   gui_plan.header.stamp = ros::Time::now();
-  for (unsigned int i = 0; i < plan.size(); i++)
+  for (int i = 0; i < plan.size(); i++)
     gui_plan.poses[i] = plan[i];
 
   // publish plan to rviz
@@ -376,6 +374,7 @@ bool GraphPlanner::_getPlanFromPath(std::vector<Node>& path, std::vector<geometr
     ROS_ERROR("This planner has not been initialized yet, but it is being used, please call initialize() before use");
     return false;
   }
+
   ros::Time planTime = ros::Time::now();
   plan.clear();
 

src/sim_env/launch/config.launch

@@ -42,7 +42,6 @@
   <include file="$(find sim_env)/launch/include/robots/start_robots.launch.xml" />
 
   <!-- open rviz for visualization -->
-  <node pkg="dynamic_rviz_config" type="rviz_generate.py" name="rosapp_rviz" args="$(arg robot_number)" output="screen"
-    if="$(eval arg('rviz_file') == '')" />
+  <node pkg="dynamic_rviz_config" type="rviz_generate.py" name="rosapp_rviz" args="$(arg robot_number)" output="screen" if="$(eval arg('rviz_file') == '')" />
   <node name="rviz" pkg="rviz" type="rviz" args="-d $(find sim_env)/rviz/$(arg rviz_file)" unless="$(eval arg('rviz_file') == '')" />
 </launch>

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

@@ -1,43 +1,22 @@
 <?xml version='1.0' encoding='utf-8'?>
 <launch>
-	<arg name="agent_number" default="4" />
-	<arg name="agent_id" default="4" />
+	<arg name="agent_number" default="1" />
+	<arg name="agent_id" default="1" />
 	<arg name="robot1_type" value="turtlebot3_waffle" />
 	<arg name="robot1_global_planner" value="a_star" />
-	<arg name="robot1_local_planner" value="rpp" />
+	<arg name="robot1_local_planner" value="pid" />
 	<arg name="robot1_x_pos" value="0.0" />
 	<arg name="robot1_y_pos" value="0.0" />
 	<arg name="robot1_z_pos" value="0.0" />
-	<arg name="robot1_yaw" value="3.14" />
-	<arg name="robot2_type" value="turtlebot3_waffle" />
-	<arg name="robot2_global_planner" value="lazy" />
-	<arg name="robot2_local_planner" value="sfm" />
-	<arg name="robot2_x_pos" value="-1.0" />
-	<arg name="robot2_y_pos" value="1.0" />
-	<arg name="robot2_z_pos" value="0.0" />
-	<arg name="robot2_yaw" value="-1.57" />
-	<arg name="robot3_type" value="turtlebot3_waffle" />
-	<arg name="robot3_global_planner" value="lazy" />
-	<arg name="robot3_local_planner" value="sfm" />
-	<arg name="robot3_x_pos" value="-1.0" />
-	<arg name="robot3_y_pos" value="-1.0" />
-	<arg name="robot3_z_pos" value="0.0" />
-	<arg name="robot3_yaw" value="0.0" />
-	<arg name="robot4_type" value="turtlebot3_waffle" />
-	<arg name="robot4_global_planner" value="lazy" />
-	<arg name="robot4_local_planner" value="sfm" />
-	<arg name="robot4_x_pos" value="1.0" />
-	<arg name="robot4_y_pos" value="-1.0" />
-	<arg name="robot4_z_pos" value="0.0" />
-	<arg name="robot4_yaw" value="1.57" />
+	<arg name="robot1_yaw" value="0.0" />
 	<include file="$(find sim_env)/launch/app/environment_single.launch.xml">
 		<arg name="agent_number" value="$(arg agent_number)" />
 		<arg name="agent_id" value="$(arg agent_id)" />
 		<arg name="robot" value="$(eval arg('robot' + str(arg('agent_id')) + '_type'))" />
 		<arg name="global_planner" value="$(eval arg('robot' + str(arg('agent_id')) + '_global_planner'))" />
 		<arg name="local_planner" value="$(eval arg('robot' + str(arg('agent_id')) + '_local_planner'))" />
 		<arg name="robot_namespace" value="robot$(arg agent_id)" />
-		<arg name="start_ns" value="true" />
+		<arg name="start_ns" value="false" />
 		<arg name="robot_x" value="$(eval arg('robot' + str(arg('agent_id')) + '_x_pos'))" />
 		<arg name="robot_y" value="$(eval arg('robot' + str(arg('agent_id')) + '_y_pos'))" />
 		<arg name="robot_z" value="$(eval arg('robot' + str(arg('agent_id')) + '_z_pos'))" />

src/sim_env/launch/main.launch

@@ -1,10 +1,11 @@
 <?xml version='1.0' encoding='utf-8'?>
 <launch>
-	<arg name="world_parameter" value="empty" />
+	<arg name="world_parameter" value="warehouse_with_pedestrians" />
+	<node pkg="dynamic_xml_config" type="obstacles_generate_ros.py" name="obstacles_generate" args="user_config.yaml" output="screen" />
 	<include file="$(find sim_env)/launch/config.launch">
 		<arg name="world" value="$(arg world_parameter)" />
 		<arg name="map" value="warehouse" />
-		<arg name="robot_number" value="4" />
-		<arg name="rviz_file" value="view_multi.rviz" />
+		<arg name="robot_number" value="1" />
+		<arg name="rviz_file" value="sim_env.rviz" />
 	</include>
 </launch>

src/sim_env/scripts/tf_map2odom.py

@@ -12,7 +12,7 @@ def __init__(self) -> None:
         self.odom_frame_id = rospy.get_param("~odom_frame_id", "odom")
 
         self.tf_pub = tf2_ros.TransformBroadcaster()
-        rospy.Timer(rospy.Duration(0.05), self.timer_callback)
+        rospy.Timer(rospy.Duration(0.1), self.timer_callback)
 
         odom = Odometry()
         odom.header.stamp = rospy.Time.now()

src/user_config/user_config.yaml

@@ -5,7 +5,7 @@ rviz_file: "sim_env.rviz"
 robots_config:
   - robot1_type: "turtlebot3_waffle"
     robot1_global_planner: "a_star"
-    robot1_local_planner: "rpp"
+    robot1_local_planner: "pid"
     robot1_x_pos: "0.0"
     robot1_y_pos: "0.0"
     robot1_z_pos: "0.0"
@@ -45,6 +45,6 @@ robots_config:
 #   * static
 
 plugins:
-  # pedestrians: "pedestrian_config.yaml"
-  # obstacles: "obstacles_config.yaml"
+  pedestrians: "pedestrian_config.yaml"
+  obstacles: "obstacles_config.yaml"
   map_layers: "map_layers_config.yaml"

src/user_config/user_config_multi.yaml

@@ -5,31 +5,31 @@ rviz_file: "view_multi.rviz"
 robots_config:
   - robot1_type: "turtlebot3_waffle"
     robot1_global_planner: "lazy"
-    robot1_local_planner: "sfm"
+    robot1_local_planner: "orca"
     robot1_x_pos: "1.0"
     robot1_y_pos: "1.0"
     robot1_z_pos: "0.0"
     robot1_yaw: "3.14"
 
   - robot2_type: "turtlebot3_waffle"
     robot2_global_planner: "lazy"
-    robot2_local_planner: "sfm"
+    robot2_local_planner: "orca"
     robot2_x_pos: "-1.0"
     robot2_y_pos: "1.0"
     robot2_z_pos: "0.0"
     robot2_yaw: "-1.57"
 
   - robot3_type: "turtlebot3_waffle"
     robot3_global_planner: "lazy"
-    robot3_local_planner: "sfm"
+    robot3_local_planner: "orca"
     robot3_x_pos: "-1.0"
     robot3_y_pos: "-1.0"
     robot3_z_pos: "0.0"
     robot3_yaw: "0.0"
 
   - robot4_type: "turtlebot3_waffle"
     robot4_global_planner: "lazy"
-    robot4_local_planner: "sfm"
+    robot4_local_planner: "orca"
     robot4_x_pos: "1.0"
     robot4_y_pos: "-1.0"
     robot4_z_pos: "0.0"