using latest mpl, add multi-robot-node

mpl_external_planner/include/mpl_external_planner/ellipsoid_planner/env_cloud.h

@@ -40,7 +40,6 @@ class env_cloud : public env_base<3> {
    * @brief Get successor
    * @param curr The node to expand
    * @param succ The array stores valid successors
-   * @param succ_idx The array stores successors' Key
    * @param succ_cost The array stores cost along valid edges
    * @param action_idx The array stores corresponding idx of control for each
    * successor
@@ -49,10 +48,9 @@ class env_cloud : public env_base<3> {
    * Here we use Heuristic function and multiply with 2
    */
   void get_succ(const Waypoint3D &curr, vec_E<Waypoint3D> &succ,
-                std::vector<Key> &succ_idx, std::vector<decimal_t> &succ_cost,
+                std::vector<decimal_t> &succ_cost,
                 std::vector<int> &action_idx) const {
     succ.clear();
-    succ_idx.clear();
     succ_cost.clear();
     action_idx.clear();
 
@@ -66,7 +64,6 @@ class env_cloud : public env_base<3> {
         continue;
       tn.t = curr.t + dt_;
       succ.push_back(tn);
-      succ_idx.push_back(state_to_idx(tn));
       succ_cost.push_back(pr.J(pr.control()) + w_ * dt_);
       action_idx.push_back(i);
     }

mpl_external_planner/include/mpl_external_planner/poly_map_planner/env_poly_map.h

@@ -33,63 +33,10 @@ class env_poly_map : public env_base<Dim> {
     return map_util_->isFree(pt, 0);
   }
 
-	///Genegrate Key from state
-	Key state_to_idx(const Waypoint<Dim>& state) const {
-		int ti = state.t/this->dt_;
-		const Veci<Dim> pi = this->round(state.pos, this->ds_);
-		if(state.control == Control::VEL)
-			return this->to_string(pi) + std::to_string(ti);
-		else if(state.control == Control::ACC) {
-			const Veci<Dim> vi = this->round(state.vel, this->dv_);
-			return this->to_string(pi) + this->to_string(vi) + std::to_string(ti);
-		}
-		else if(state.control == Control::JRK) {
-			const Veci<Dim> vi = this->round(state.vel, this->dv_);
-			const Veci<Dim> ai = this->round(state.acc, this->da_);
-			return this->to_string(pi) + this->to_string(vi) + this->to_string(ai) +
-				std::to_string(ti);
-		}
-		else if(state.control == Control::SNP) {
-			const Veci<Dim> vi = this->round(state.vel, this->dv_);
-			const Veci<Dim> ai = this->round(state.acc, this->da_);
-			const Veci<Dim> ji = this->round(state.jrk, this->dj_);
-			return this->to_string(pi) + this->to_string(vi) + this->to_string(ai) +
-				this->to_string(ji) + std::to_string(ti);
-		}
-		else if(state.control == Control::VELxYAW) {
-			int yawi = std::round(state.yaw/this->dyaw_);
-			return this->to_string(pi) + std::to_string(yawi) + "-" + std::to_string(ti);
-		}
-		else if(state.control == Control::ACCxYAW) {
-			const Veci<Dim> vi = this->round(state.vel, this->dv_);
-			int yawi = std::round(state.yaw/this->dyaw_);
-			return this->to_string(pi) + this->to_string(vi) +
-				std::to_string(yawi) + "-" + std::to_string(ti);
-		}
-		else if(state.control == Control::JRKxYAW) {
-			const Veci<Dim> vi = this->round(state.vel, this->dv_);
-			const Veci<Dim> ai = this->round(state.acc, this->da_);
-			int yawi = std::round(state.yaw/this->dyaw_);
-			return this->to_string(pi) + this->to_string(vi) + this->to_string(ai) +
-				std::to_string(yawi) + "-" + std::to_string(ti);
-		}
-		else if(state.control == Control::SNPxYAW) {
-			const Veci<Dim> vi = this->round(state.vel, this->dv_);
-			const Veci<Dim> ai = this->round(state.acc, this->da_);
-			const Veci<Dim> ji = this->round(state.jrk, this->dj_);
-			int yawi = std::round(state.yaw/this->dyaw_);
-			return this->to_string(pi) + this->to_string(vi) + this->to_string(ai) +
-        this->to_string(ji) + std::to_string(yawi) + "-" + std::to_string(ti);
-		}
-		else
-			return std::to_string(ti);
-	}
-
   /**
    * @brief Get successor
    * @param curr The node to expand
    * @param succ The array stores valid successors
-   * @param succ_idx The array stores successors' Key
    * @param succ_cost The array stores cost along valid edges
    * @param action_idx The array stores corresponding idx of control for each
    * successor
@@ -98,10 +45,9 @@ class env_poly_map : public env_base<Dim> {
 	 * Here we use Heuristic function and multiply with 2
 	 */
 	void get_succ(const Waypoint<Dim> &curr, vec_E<Waypoint<Dim>> &succ,
-								std::vector<Key> &succ_idx, std::vector<decimal_t> &succ_cost,
+								std::vector<decimal_t> &succ_cost,
 								std::vector<int> &action_idx) const {
 		succ.clear();
-		succ_idx.clear();
 		succ_cost.clear();
 		action_idx.clear();
 
@@ -114,9 +60,9 @@ class env_poly_map : public env_base<Dim> {
 			if(!map_util_->isFree(pr, curr.t))
 				continue;
 			tn.t = curr.t + this->dt_;
+      tn.enable_t = true;
 			succ.push_back(tn);
-			succ_idx.push_back(state_to_idx(tn));
-			succ_cost.push_back(pr.J(pr.control()) + this->w_ * this->dt_);
+			succ_cost.push_back(pr.J(pr.control()) + 0.001 * pr.J(Control::VEL) + this->w_ * this->dt_);
 			action_idx.push_back(i);
 		}
 	}

mpl_test_node/launch/multi_robot_node/test.launch

@@ -11,11 +11,12 @@
     launch-prefix="$(arg prefix)"
     output="screen">
     <!-- Set dynamic constraints -->
-    <param name="v_max" value="1.0"/>
+    <param name="v_max" value="2.0"/>
     <param name="a_max" value="1.0"/>
-    <param name="u" value="0.5"/>
-    <param name="dt" value="1.0"/>
+    <param name="u" value="1.0"/>
+    <param name="dt" value="0.5"/>
     <param name="ndt" value="0"/>
+    <param name="file" value="$(find mpl_test_node)/launch/multi_robot_node/sim.bag"/>
   </node>
 
 </launch>

mpl_test_node/launch/multi_robot_node/test.rviz

@@ -55,54 +55,6 @@ Visualization Manager:
       Radius: 0.100000001
       Reference Frame: <Fixed Frame>
       Value: true
-    - AccColor: 10; 200; 55
-      AccScale: 0.0199999996
-      AccVis: false
-      Class: planning_rviz_plugins/Trajectory
-      Enabled: true
-      JrkColor: 200; 20; 55
-      JrkScale: 0.0199999996
-      JrkVis: false
-      Name: Trajectory
-      Num of samples: 200
-      NumYaw: 20
-      PosColor: 255; 0; 0
-      PosScale: 0.0500000007
-      Topic: /test_primitive/trajectory1
-      Unreliable: false
-      Value: true
-      VelColor: 85; 85; 255
-      VelScale: 0.0199999996
-      VelVis: false
-      YawColor: 100; 20; 55
-      YawScale: 0.0500000007
-      YawTriangleAngle: 0.699999988
-      YawTriangleScale: 0.5
-      YawVis: false
-    - AccColor: 10; 200; 55
-      AccScale: 0.0199999996
-      AccVis: false
-      Class: planning_rviz_plugins/Trajectory
-      Enabled: true
-      JrkColor: 200; 20; 55
-      JrkScale: 0.0199999996
-      JrkVis: false
-      Name: Trajectory
-      Num of samples: 100
-      NumYaw: 20
-      PosColor: 204; 51; 204
-      PosScale: 0.100000001
-      Topic: /test_primitive/trajectory2
-      Unreliable: false
-      Value: true
-      VelColor: 85; 85; 255
-      VelScale: 0.0199999996
-      VelVis: false
-      YawColor: 100; 20; 55
-      YawScale: 0.0500000007
-      YawTriangleAngle: 0.699999988
-      YawTriangleScale: 0.5
-      YawVis: false
     - Alpha: 1
       Autocompute Intensity Bounds: true
       Autocompute Value Bounds:
@@ -128,19 +80,11 @@ Visualization Manager:
       Size (Pixels): 3
       Size (m): 0.400000006
       Style: Spheres
-      Topic: /test_primitive/states
+      Topic: /states
       Unreliable: false
       Use Fixed Frame: true
       Use rainbow: true
       Value: true
-    - Alpha: 0.5
-      Class: decomp_rviz_plugins/EllipsoidArray
-      Color: 204; 51; 204
-      Enabled: true
-      Name: EllipsoidArray
-      Topic: /test_primitive/ellipsoids
-      Unreliable: false
-      Value: true
     - Alpha: 0.200000003
       BoundColor: 255; 0; 0
       Class: decomp_rviz_plugins/PolyhedronArray
@@ -149,7 +93,7 @@ Visualization Manager:
       Name: PolyhedronArray
       Scale: 0.100000001
       State: Mesh
-      Topic: /test_primitive/polyhedrons
+      Topic: polyhedrons
       Unreliable: false
       Value: true
       VsColor: 0; 255; 0
@@ -167,6 +111,41 @@ Visualization Manager:
       Value: true
       VsColor: 0; 255; 0
       VsScale: 1
+    - Class: planning_rviz_plugins/PathArray
+      Enabled: true
+      ID: All
+      LineColor: 0; 0; 255
+      LineScale: 0.0500000007
+      Name: PathArray
+      NodeColor: 85; 85; 255
+      NodeScale: 0
+      Topic: paths
+      Unreliable: false
+      Value: true
+    - AccColor: 10; 200; 55
+      AccScale: 0.0199999996
+      AccVis: false
+      Class: planning_rviz_plugins/PrimitiveArray
+      Enabled: true
+      JrkColor: 200; 20; 55
+      JrkScale: 0.0199999996
+      JrkVis: false
+      Name: PrimitiveArray
+      Num: 10
+      NumYaw: 2
+      PosColor: 204; 51; 204
+      PosScale: 0.0500000007
+      Topic: prs
+      Unreliable: false
+      Value: true
+      VelColor: 85; 85; 255
+      VelScale: 0.0199999996
+      VelVis: false
+      YawColor: 100; 20; 55
+      YawScale: 0.0500000007
+      YawTriangleAngle: 0.699999988
+      YawTriangleScale: 0.5
+      YawVis: false
   Enabled: true
   Global Options:
     Background Color: 255; 255; 255
@@ -201,11 +180,11 @@ Visualization Manager:
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.00999999978
-      Scale: 43.6787796
+      Scale: 68.7294083
       Target Frame: <Fixed Frame>
       Value: TopDownOrtho (rviz)
-      X: 10.9398279
-      Y: 2.73615527
+      X: 5.3621583
+      Y: -0.131871775
     Saved: ~
 Window Geometry:
   Displays:

mpl_test_node/launch/poly_map_planner_node/test.launch

@@ -9,16 +9,18 @@
     type="poly_map_planner_node"
     name="test_primitive"
     launch-prefix="$(arg prefix)"
+    clear_params="true"
     output="screen">
     <!-- Set start and goal -->
-    <param name="goal_x" value="18"/>
-    <param name="goal_y" value="5"/>
+    <param name="goal_x" value="10.5"/>
+    <param name="goal_y" value="2.5"/>
     <param name="start_vx" value="0.0"/>
     <param name="start_vy" value="0.0"/>
     <param name="start_x" value="4.0"/>
     <param name="start_y" value="2.5"/>
+    <!--<param name="origin_x" value="2.0"/>-->
     <!-- Set dynamic constraints -->
-    <param name="v_max" value="1.0"/>
+    <param name="v_max" value="2.0"/>
     <param name="a_max" value="1.0"/>
     <param name="u" value="0.5"/>
     <param name="dt" value="1.0"/>

mpl_test_node/src/bag_writter.hpp

@@ -0,0 +1,17 @@
+#include <ros/console.h>
+#include <rosbag/bag.h>
+#include <rosbag/view.h>
+
+#include <boost/foreach.hpp>
+
+template<typename T>
+void write_bag(std::string file_name, std::string topic_name, const std::vector<T>& msgs) {
+  rosbag::Bag bag;
+  bag.open(file_name, rosbag::bagmode::Write);
+
+  for(const auto& it: msgs)
+    bag.write(topic_name, it.header.stamp, it);
+  ROS_INFO("Write %zu messages as %s in file %s",
+           msgs.size(), topic_name.c_str(), file_name.c_str());
+  bag.close();
+}