Get it to work

CMakeLists.txt

@@ -1,8 +1,10 @@
 cmake_minimum_required(VERSION 2.8.3)
 project(iarc7_planner)
 
+set(CMAKE_BUILD_TYPE Release)
+
 set(CMAKE_CXX_FLAGS "-Wall -Wextra -Werror ${CMAKE_CXX_FLAGS}")
-set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -DROS_ASSERT_ENABLED")
+set(CMAKE_CXX_FLAGS_RELEASE"${CMAKE_CXX_FLAGS_RELEASE} -DROS_ASSERT_ENABLED")
 
 ## Find catkin macros and libraries
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
@@ -19,7 +21,6 @@ find_package(catkin REQUIRED COMPONENTS
   tf2_ros
   tf2_geometry_msgs
   motion_primitive_library
-  decomp_ros_utils
   planning_ros_utils
   planning_ros_msgs
 )
@@ -114,9 +115,9 @@ find_package(catkin REQUIRED COMPONENTS
 ## DEPENDS: system dependencies of this project that dependent projects also need
  catkin_package(
 #   INCLUDE_DIRS include
-#  LIBRARIES 
-#  CATKIN_DEPENDS 
-#  DEPENDS 
+#  LIBRARIES
+#  CATKIN_DEPENDS
+#  DEPENDS
  )
 
 ###########

include/MapGenerator.hpp

@@ -0,0 +1,40 @@
+////////////////////////////////////////////////////////////////////////////
+//
+// Map Generator
+//
+// Generates maps from obstacle information
+//
+////////////////////////////////////////////////////////////////////////////
+
+#ifndef MAP_GENERATOR_H
+#define MAP_GENERATOR_H
+
+#include <ros/ros.h>
+
+// ROS message headers
+#include "iarc7_msgs/MotionPointStamped.h"
+#include "iarc7_msgs/ObstacleArray.h"
+
+#include <planning_ros_utils/voxel_grid.h>
+#include <planning_ros_msgs/VoxelMap.h>
+#include <planning_ros_utils/data_ros_utils.h>
+#include <planning_ros_utils/primitive_ros_utils.h>
+#include <motion_primitive_library/planner/mp_map_util.h>
+
+#include <iarc7_msgs/ObstacleArray.h>
+namespace Iarc7Planner
+{
+
+class MapGenerator
+{
+public:
+    MapGenerator(ros::NodeHandle nh);
+
+    getMap()
+
+private:
+};
+
+} // End namespace Iarc7Planner
+
+#endif // MAP_GENERATOR_H

launch/planner.launch

@@ -3,13 +3,12 @@
     <arg name="bond_id_namespace" default="safety_bonds" />
 
     <node name="motion_planner" pkg="iarc7_planner"
-        type="motion_planner">
+        type="motion_planner" >
+
         <rosparam command="load"
             file="$(find iarc7_planner)/param/platform_$(arg platform).yaml" />
         <rosparam command="load"
             file="$(find iarc7_planner)/param/planner.yaml" />
     </node>
 
-    <param name="$(arg bond_id_namespace)/motion_planner/form_bond"
-        value="true" />
 </launch>

launch/rviz.launch

@@ -0,0 +1,9 @@
+<launch>
+  <arg name="test" default="test_primitive_map"/>
+
+  <node pkg="rviz"
+    type="rviz"
+    name="rviz"
+    output="screen"
+    args="-d $(find mpl_test_node)/launch/$(arg test)/test.rviz"/>
+</launch>

package.xml

@@ -4,7 +4,7 @@
   <version>0.0.0</version>
   <description>The iarc7_planner package</description>
 
-  <!-- One maintainer tag required, multiple allowed, one person per tag --> 
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
   <!-- Example:  -->
   <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
   <maintainer email="ras@pitt.edu">Pitt Robotics and Automation Society</maintainer>
@@ -48,7 +48,6 @@
   <build_depend>tf2_ros</build_depend>
   <build_depend>tf2_geometry_msgs</build_depend>
   <build_depend>iarc7_safety</build_depend>
-  <build_depend>decomp_ros_utils</build_depend>
   <build_depend>planning_ros_utils</build_depend>
   <build_depend>planning_ros_msgs</build_depend>
   <run_depend>iarc7_msgs</run_depend>

param/planner.yaml

@@ -1,9 +1,9 @@
-# Params for planner
+# PARAMS FOR GRAPH SEARCH PLANNER
 
-# Timeout for transforms
-transform_timeout       : 0.2
+# timeout for getting a successful plan
+planning_timeout: .100
 
-# update frequency 
+# update frequency
 planner_update_frequency : 60.0
 
 # The flags use_xxx will adapt the planning in different control space
@@ -19,3 +19,25 @@ use_pos: true
 use_vel: true
 use_acc: true
 use_jrk: false
+
+# min/max time discretization for each primitive
+min_dt: 0.10
+max_dt: 0.25
+
+# max number of time discretization for a plan
+ndt: 1000
+
+# epsilon
+eps: 10.0
+
+# control discretization
+num: 1
+
+# maximum number of allowed expansion; -1 means no limit
+max_num: 1500
+
+# resolution to generate map at
+map_res: .25
+
+# samples per sec for generating waypoints
+samples_per_sec: 50.0

param/platform_2.0.yaml

@@ -0,0 +1,13 @@
+# Params for drone size for planner
+radius: 0.6 # meters
+buffer: 2.0 # meters
+
+# kinematic constraints
+max_speed: 0.75 # m/s
+max_acceleration: 1.0 # m/s^2
+max_jerk: 4.0 # m/s^3
+
+# tolerances
+p_tol: 0.1
+vel_tol: 0.15
+accel_tol: 0.25

param/platform_crazyflie.yaml

@@ -0,0 +1,13 @@
+# Params for drone size for planner
+radius: 0.6 # meters
+buffer: 0.1 # meters
+
+# kinematic constraints
+max_speed: 0.75 # m/s
+max_acceleration: 1.0 # m/s^2
+max_jerk: 4.0 # m/s^3
+
+# tolerances
+p_tol: 0.1
+vel_tol: 0.15
+accel_tol: 0.25

param/platform_sim.yaml

@@ -1,11 +1,13 @@
 # Params for drone size for planner
-radius: 1.5 # meters
-buffer: 0.2 # meters
+radius: 0.6 # meters
+buffer: 0.1 # meters
 
 # kinematic constraints
-max_speed: 3.0 # m/s
-max_acceleration: 5.0 # m/s^2
-max_jerk: 15.0 # m/s^3
+max_speed: 0.75 # m/s
+max_acceleration: 1.0 # m/s^2
+max_jerk: 4.0 # m/s^3
 
-# Minimum height for any XY translationanl maneuvers, meters
-min_maneuver_height: 0.3 
+# tolerances
+p_tol: 0.1
+vel_tol: 0.15
+accel_tol: 0.25