simplify multi_robot_node using robot & robot_team classes

README.md

@@ -85,8 +85,8 @@ We can build a team of robots that move simultaneously in a constrained environm
 In the following demo, each robot replans constantly at 2Hz and it runs its own planner that tries to reach the pre-allocate goal as fast as possible.
 We assume the robot knows other robots' trajectory at the time when it's planning.
 
-Star  | Tunnel
-:---- | :----
+Config1: 10 robots | Config2: 16 robots
+:----------------- | :-----------------
 <img src="./mpl_test_node/samples/sample3.gif" width="328"> | <img src="./mpl_test_node/samples/sample4.gif" width="328">
 
 

mpl_test_node/CMakeLists.txt

@@ -30,10 +30,8 @@ target_link_libraries(map_replanner_node ${MOTION_PRIMITIVE_LIBRARY_LIBRARIES})
 cs_add_executable(traj_solver_node src/traj_solver_node.cpp)
 target_link_libraries(traj_solver_node ${MOTION_PRIMITIVE_LIBRARY_LIBRARIES})
 
-cs_add_executable(multi_robot_node1 src/multi_robot_node1.cpp)
-target_link_libraries(multi_robot_node1 ${MOTION_PRIMITIVE_LIBRARY_LIBRARIES})
-cs_add_executable(multi_robot_node2 src/multi_robot_node2.cpp)
-target_link_libraries(multi_robot_node2 ${MOTION_PRIMITIVE_LIBRARY_LIBRARIES})
+cs_add_executable(multi_robot_node src/multi_robot_node.cpp)
+target_link_libraries(multi_robot_node ${MOTION_PRIMITIVE_LIBRARY_LIBRARIES})
 
 cs_add_executable(nonlinear_obstacle_node src/nonlinear_obstacle_node.cpp)
 target_link_libraries(nonlinear_obstacle_node ${MOTION_PRIMITIVE_LIBRARY_LIBRARIES})

mpl_test_node/launch/multi_robot_node/test.launch

@@ -1,6 +1,5 @@
 <launch>
   <arg name="debug" default="false"/>
-  <arg name="example" default="1"/>
   <!--<arg name="debug_valgrind" default="false"/>-->
 
   <arg name="prefix" value="" unless="$(arg debug)"/>
@@ -9,7 +8,7 @@
   <param name="use_sim_time" value="false" />
 
   <node pkg="mpl_test_node"
-    type="multi_robot_node$(arg example)"
+    type="multi_robot_node"
     name="test_primitive"
     launch-prefix="$(arg prefix)"
     output="screen">
@@ -19,6 +18,8 @@
     <param name="u" value="1.0"/>
     <param name="dt" value="0.5"/>
     <param name="ndt" value="0"/>
+    <param name="use_config1" value="false"/>
+    <param name="use_config2" value="true"/>
     <param name="file" value="$(find mpl_test_node)/launch/multi_robot_node/sim.bag"/>
   </node>
 

mpl_test_node/launch/multi_robot_node/test.rviz

@@ -210,11 +210,11 @@ Visualization Manager:
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.00999999978
-      Scale: 68.7294083
+      Scale: 75.4797592
       Target Frame: <Fixed Frame>
       Value: TopDownOrtho (rviz)
-      X: 5.3621583
-      Y: -0.131871775
+      X: 4.92495584
+      Y: -0.105376266
     Saved: ~
 Window Geometry:
   Displays:
@@ -232,5 +232,5 @@ Window Geometry:
   Views:
     collapsed: false
   Width: 1920
-  X: 3840
-  Y: 24
+  X: 1920
+  Y: 48

mpl_test_node/src/multi_robot_node.cpp

@@ -0,0 +1,205 @@
+/**
+ * @brief Multi-robot test node in a tunnel case
+ */
+#include "bag_writter.hpp"
+#include "robot_team.hpp"
+#include <decomp_ros_utils/data_ros_utils.h>
+#include <planning_ros_utils/data_ros_utils.h>
+#include <planning_ros_utils/primitive_ros_utils.h>
+#include <ros/ros.h>
+
+using namespace MPL;
+
+int main(int argc, char **argv) {
+  ros::init(argc, argv, "test");
+  ros::NodeHandle nh("~");
+
+  std::string file_name;
+  std::string states_name, starts_name, polys_name, paths_name, prs_name;
+  nh.param("file", file_name, std::string("sim.bag"));
+  nh.param("states_name", states_name, std::string("/states"));
+  nh.param("starts_name", starts_name, std::string("/starts"));
+  nh.param("polys_name", polys_name, std::string("/polyhedrons"));
+  nh.param("paths_name", paths_name, std::string("/paths"));
+  nh.param("prs_name", prs_name, std::string("/prs"));
+
+  ros::Publisher poly_pub =
+    nh.advertise<decomp_ros_msgs::PolyhedronArray>(polys_name, 1, true);
+  ros::Publisher bound_pub =
+    nh.advertise<decomp_ros_msgs::PolyhedronArray>("bound", 1, true);
+  ros::Publisher state_pub =
+      nh.advertise<sensor_msgs::PointCloud>(states_name, 1, true);
+  ros::Publisher start_pub =
+      nh.advertise<sensor_msgs::PointCloud>(starts_name, 1, true);
+  ros::Publisher prs_pub =
+      nh.advertise<planning_ros_msgs::PrimitiveArray>(prs_name, 1, true);
+  ros::Publisher path_pub =
+      nh.advertise<planning_ros_msgs::PathArray>(paths_name, 1, true);
+
+  Vec2f origin, dim;
+  nh.param("origin_x", origin(0), 0.0);
+  nh.param("origin_y", origin(1), -5.0);
+  nh.param("range_x", dim(0), 10.0);
+  nh.param("range_y", dim(1), 10.0);
+
+  // Initialize planner
+  double dt, v_max, a_max, t_max;
+  double u;
+  int num;
+  nh.param("dt", dt, 1.0);
+  nh.param("v_max", v_max, -1.0);
+  nh.param("a_max", a_max, -1.0);
+  nh.param("u", u, 1.0);
+  nh.param("num", num, 1);
+
+  // Set input control
+  vec_E<VecDf> U;
+  const decimal_t du = u / num;
+  for (decimal_t dx = -u; dx <= u; dx += du)
+    for (decimal_t dy = -u; dy <= u; dy += du)
+      U.push_back(Vec2f(dx, dy));
+
+  bool use_config1, use_config2;
+  nh.param("use_config1", use_config1, true);
+  nh.param("use_config2", use_config2, false);
+
+  // Set robot geomtric shape
+  Polyhedron2D rec;
+  rec.add(Hyperplane2D(Vec2f(-0.5, 0), -Vec2f::UnitX()));
+  rec.add(Hyperplane2D(Vec2f(0.5, 0), Vec2f::UnitX()));
+  rec.add(Hyperplane2D(Vec2f(0, -0.5), -Vec2f::UnitY()));
+  rec.add(Hyperplane2D(Vec2f(0, 0.5), Vec2f::UnitY()));
+
+  std::unique_ptr<HomogeneousRobotTeam<2>> robot_team;
+  if(use_config1) {
+    robot_team.reset(new Team1(0.01));
+  }
+  else if(use_config2)
+    robot_team.reset(new Team2(0.01));
+  robot_team->set_v_max(v_max);
+  robot_team->set_a_max(a_max);
+  robot_team->set_u(U);
+  robot_team->set_dt(dt);
+  robot_team->set_map(origin, dim);
+  robot_team->set_geometry(rec);
+
+  robot_team->init();
+
+  vec_E<Polyhedron2D> bbox;
+  bbox.push_back(robot_team->get_robots().front()->get_bbox());
+  decomp_ros_msgs::PolyhedronArray bbox_msg = DecompROS::polyhedron_array_to_ros(bbox);
+  bbox_msg.header.frame_id = "map";
+  bbox_msg.header.stamp = ros::Time::now();
+  bound_pub.publish(bbox_msg);
+
+  // Start the replan loop
+  ros::Rate loop_rate(100);
+  decimal_t update_t = 0.01;
+  decimal_t time = 0;
+  decimal_t prev_time = -1000;
+  ros::Time t0 = ros::Time::now();
+
+  std::vector<sensor_msgs::PointCloud> state_msgs;
+  std::vector<sensor_msgs::PointCloud> start_msgs;
+  std::vector<decomp_ros_msgs::PolyhedronArray> poly_msgs;
+  std::vector<planning_ros_msgs::PathArray> path_msgs;
+  std::vector<planning_ros_msgs::PrimitiveArray> prs_msgs;
+  while (ros::ok()) {
+    time += update_t;
+
+    // set obstacle simultaneously
+    auto poly_obs = robot_team->set_obs(time); // set obstacles at time
+
+    // plan
+    if(!robot_team->plan(time)) {
+      ROS_INFO("Robot fails to plan, ABORT!");
+      break;
+    }
+
+    // Visualizing current status at 10 Hz
+    if(time - prev_time >= 0.1) {
+      // Reduce the size of obstacles manually.
+      for(auto& it: poly_obs) {
+        for(auto& itt: it.vs_)
+          itt.p_ -= itt.n_ * 0.25;
+      }
+
+      decomp_ros_msgs::PolyhedronArray poly_msg = DecompROS::polyhedron_array_to_ros(poly_obs);
+      poly_msg.header.frame_id = "map";
+      poly_msg.header.stamp = t0 + ros::Duration(time);
+      poly_pub.publish(poly_msg);
+      poly_msgs.push_back(poly_msg);
+
+      vec_E<vec_Vec3f> path_array;
+      vec_Vec2f states;
+      vec_Vec2f starts;
+      for(auto& it: robot_team->get_robots()) {
+        starts.push_back(it->get_start().pos);
+        states.push_back(it->get_state(time).pos);
+        path_array.push_back(vec2_to_vec3(it->get_history()));
+      }
+
+      auto path_msg = path_array_to_ros(path_array);
+      path_msg.header.frame_id = "map";
+      path_msg.header.stamp = t0 + ros::Duration(time);
+      path_pub.publish(path_msg);
+      path_msgs.push_back(path_msg);
+
+      auto state_msg = vec_to_cloud(vec2_to_vec3(states));
+      state_msg.header.frame_id = "map";
+      state_msg.header.stamp = t0 + ros::Duration(time);
+      state_pub.publish(state_msg);
+      state_msgs.push_back(state_msg);
+
+      auto start_msg = vec_to_cloud(vec2_to_vec3(starts));
+      start_msg.header.frame_id = "map";
+      start_msg.header.stamp = t0 + ros::Duration(time);
+      start_pub.publish(start_msg);
+      start_msgs.push_back(start_msg);
+
+      vec_E<Primitive2D> prs_array;
+      for(auto& it: robot_team->get_robots()) {
+        auto prs = it->get_primitives();
+        prs_array.insert(prs_array.end(), prs.begin(), prs.end());
+      }
+
+      auto prs_msg = toPrimitiveArrayROSMsg(prs_array);
+      prs_msg.header.frame_id = "map";
+      prs_msg.header.stamp = t0 + ros::Duration(time);
+      prs_pub.publish(prs_msg);
+      prs_msgs.push_back(prs_msg);
+
+      prev_time = time;
+    }
+
+    if(robot_team->finished(time)) {
+      ROS_INFO("All robots reached!");
+      break;
+    }
+
+    ros::spinOnce();
+
+    loop_rate.sleep();
+  }
+
+  ROS_WARN("Total time: %f", (ros::Time::now() - t0).toSec());
+
+  // Write to bag (optional)
+  rosbag::Bag bag;
+  bag.open(file_name, rosbag::bagmode::Write);
+
+  for(const auto& it: start_msgs)
+    bag.write(starts_name, it.header.stamp, it);
+  for(const auto& it: state_msgs)
+    bag.write(states_name, it.header.stamp, it);
+  for(const auto& it: poly_msgs)
+    bag.write(polys_name, it.header.stamp, it);
+  for(const auto& it: path_msgs)
+    bag.write(paths_name, it.header.stamp, it);
+  for(const auto& it: prs_msgs)
+    bag.write(prs_name, it.header.stamp, it);
+
+  bag.close();
+
+  return 0;
+}