Merge pull request #160 from KumarRobotics/feature/remove_planning_ro…

…s_msgs Move planning_ros_msgs to be an external dependency
KumarRobotics · May 29, 2023 · c51e64e · c51e64e
2 parents 5a95942 + e373c76
commit c51e64e
Show file tree

Hide file tree

Showing 103 changed files with 442 additions and 3,711 deletions.
diff --git a/.github/workflows/cpplint-reviewdog.yaml b/.github/workflows/cpplint-reviewdog.yaml
@@ -5,12 +5,12 @@ jobs:
   cpplint:
     runs-on: ubuntu-latest
     steps:
-    - uses: actions/checkout@master
-    - uses: reviewdog/action-cpplint@master
-      with:
-        github_token: ${{ secrets.KR_AUTONOMOUS_FLIGHT_TOKEN_REVIEWDOG }}
-        reporter: github-pr-check
-        flags: --exclude=autonomy_core/map_plan/jps3d/include/jps/graph_search.h --exclude autonomy_core/map_plan/jps3d/src/graph_search.cpp --exclude autonomy_core/map_plan/planning_ros_utils/src/planning_rviz_plugins/map_display.h --exclude autonomy_core/state_machine/action_trackers/src/take_off_tracker.cpp --exclude autonomy_core/map_plan/mpl/include/mpl_basis/lambda.h --exclude autonomy_core/map_plan/mpl/include/mpl_planner/env_base.h --exclude autonomy_core/map_plan/mpl/src/env_base.cpp --exclude autonomy_core/map_plan/mpl/src/env_map.cpp
-        filter: "-whitespace/comments,-whitespace/indent,-build/include_order,-whitespace/ending_newline,-runtime/references"
-        # Note: runtime/references was added to address google benchmark
-        # issues, as requested by https://github.com/KumarRobotics/kr_autonomous_flight/pull/171
+      - uses: actions/checkout@master
+      - uses: reviewdog/action-cpplint@master
+        with:
+          github_token: ${{ secrets.KR_AUTONOMOUS_FLIGHT_TOKEN_REVIEWDOG }}
+          reporter: github-pr-check
+          flags: --exclude=autonomy_core/map_plan/jps3d/include/jps/graph_search.h --exclude autonomy_core/map_plan/jps3d/src/graph_search.cpp --exclude autonomy_core/state_machine/action_trackers/src/take_off_tracker.cpp --exclude autonomy_core/map_plan/mpl/include/mpl_basis/lambda.h --exclude autonomy_core/map_plan/mpl/include/mpl_planner/env_base.h --exclude autonomy_core/map_plan/mpl/src/env_base.cpp --exclude autonomy_core/map_plan/mpl/src/env_map.cpp
+          filter: "-whitespace/comments,-whitespace/indent,-build/include_order,-whitespace/ending_newline,-runtime/references"
+          # Note: runtime/references was added to address google benchmark
+          # issues, as requested by https://github.com/KumarRobotics/kr_autonomous_flight/pull/171
diff --git a/.gitignore b/.gitignore
@@ -4,6 +4,7 @@ mrsl_quadrotor/
 msckf_vio/
 ouster_example/
 waypoint_navigation_plugin/
+kr_planning_msgs/
 
 #===========
 # C++

diff --git a/autonomy_core/client/client_launch/rviz/client.rviz b/autonomy_core/client/client_launch/rviz/client.rviz
@@ -76,8 +76,7 @@ Visualization Manager:
       Show Arrows: true
       Show Axes: true
       Show Names: true
-      Tree:
-        {}
+      Tree: {}
       Update Interval: 0
       Value: false
     - Class: rviz/InteractiveMarkers
@@ -95,15 +94,14 @@ Visualization Manager:
           Enabled: true
           Marker Topic: /env_vis/env
           Name: Environment
-          Namespaces:
-            {}
+          Namespaces: {}
           Queue Size: 100
           Value: true
       Enabled: false
       Name: Env
     - Class: rviz/Group
       Displays:
-        - Class: planning_rviz_plugins/Path
+        - Class: kr_planning_rviz_plugins/Path
           Enabled: true
           LineColor: 204; 51; 204
           LineScale: 0.4000000059604645
@@ -114,7 +112,7 @@ Visualization Manager:
           Topic: /quadrotor/global_plan_server/path
           Unreliable: false
           Value: true
-        - Class: planning_rviz_plugins/Path
+        - Class: kr_planning_rviz_plugins/Path
           Enabled: true
           LineColor: 0; 255; 0
           LineScale: 0.800000011920929
@@ -153,7 +151,7 @@ Visualization Manager:
           Use Fixed Frame: true
           Use rainbow: true
           Value: true
-        - Class: planning_rviz_plugins/Path
+        - Class: kr_planning_rviz_plugins/Path
           Enabled: true
           LineColor: 78; 154; 6
           LineScale: 0.800000011920929
@@ -167,7 +165,7 @@ Visualization Manager:
         - AccColor: 10; 200; 55
           AccScale: 0.014999999664723873
           AccVis: true
-          Class: planning_rviz_plugins/Trajectory
+          Class: kr_planning_rviz_plugins/Trajectory
           Enabled: true
           JrkColor: 200; 20; 55
           JrkScale: 0.014999999664723873
@@ -189,7 +187,7 @@ Visualization Manager:
           YawTriangleAngle: 0.699999988079071
           YawTriangleScale: 0.5
           YawVis: true
-        - Class: planning_rviz_plugins/Path
+        - Class: kr_planning_rviz_plugins/Path
           Enabled: true
           LineColor: 0; 170; 255
           LineScale: 0.5
@@ -222,7 +220,7 @@ Visualization Manager:
           Axis: Z
           BoundScale: 0.5
           Channel Name: intensity
-          Class: planning_rviz_plugins/VoxelMap
+          Class: kr_planning_rviz_plugins/VoxelMap
           Color: 239; 41; 41
           Color Transformer: AxisColor
           Decay Time: 0
@@ -255,7 +253,7 @@ Visualization Manager:
           Axis: Z
           BoundScale: 3
           Channel Name: intensity
-          Class: planning_rviz_plugins/VoxelMap
+          Class: kr_planning_rviz_plugins/VoxelMap
           Color: 252; 233; 79
           Color Transformer: FlatColor
           Decay Time: 0

diff --git a/autonomy_core/client/rqt_quadrotor_safety/package.xml b/autonomy_core/client/rqt_quadrotor_safety/package.xml
@@ -14,7 +14,7 @@
   <buildtool_depend>catkin</buildtool_depend>
 
   <run_depend>geometry_msgs</run_depend>
-  <run_depend>planning_ros_msgs</run_depend>
+  <run_depend>kr_planning_msgs</run_depend>
   <run_depend>mavros_msgs</run_depend>
   <run_depend>python-rospkg</run_depend>
   <run_depend>rostopic</run_depend>

diff --git a/autonomy_core/client/rqt_quadrotor_safety/src/rqt_quadrotor_safety/quadrotor_safety.py b/autonomy_core/client/rqt_quadrotor_safety/src/rqt_quadrotor_safety/quadrotor_safety.py
@@ -18,7 +18,7 @@
 from rqt_gui_py.plugin import Plugin
 
 import std_msgs.msg
-import planning_ros_msgs.msg as MHL
+import kr_planning_msgs.msg as MHL
 from nav_msgs.msg import Odometry
 import kr_mav_msgs.msg as QM
 import numpy as np

diff --git a/autonomy_core/map_plan/action_planner/CMakeLists.txt b/autonomy_core/map_plan/action_planner/CMakeLists.txt
@@ -15,17 +15,17 @@ find_package(
              pcl_ros
              mapper
              jps3d
-             planning_ros_utils
-             planning_ros_msgs
+             kr_planning_rviz_plugins
+             kr_planning_msgs
              traj_opt_ros
              dynamic_reconfigure
              motion_primitive_library)
 
 generate_dynamic_reconfigure_options(cfg/ActionPlanner.cfg)
 
-catkin_package(CATKIN_DEPENDS dynamic_reconfigure planning_ros_msgs)
+catkin_package(CATKIN_DEPENDS dynamic_reconfigure kr_planning_msgs)
 
-add_library(${PROJECT_NAME} src/data_conversions.cpp)
+add_library(${PROJECT_NAME} src/data_conversions.cpp src/primitive_ros_utils.cpp)
 target_include_directories(${PROJECT_NAME} PUBLIC src ${catkin_INCLUDE_DIRS})
 target_link_libraries(${PROJECT_NAME} PUBLIC ${catkin_LIBRARIES})
 

diff --git a/autonomy_core/map_plan/action_planner/package.xml b/autonomy_core/map_plan/action_planner/package.xml
@@ -26,8 +26,8 @@
   <depend>pcl_ros</depend>
   <depend>mapper</depend>
   <depend>jps3d</depend>
-  <depend>planning_ros_utils</depend>
-  <depend>planning_ros_msgs</depend>
+  <depend>kr_planning_rviz_plugins</depend>
+  <depend>kr_planning_msgs</depend>
   <depend>traj_opt_ros</depend>
   <depend>motion_primitive_library</depend>
 

diff --git a/autonomy_core/map_plan/action_planner/src/data_conversions.cpp b/autonomy_core/map_plan/action_planner/src/data_conversions.cpp
@@ -1,82 +1,85 @@
-#include "data_conversions.h"
+#include <data_conversions.h>
 
-void setMap(std::shared_ptr<MPL::VoxelMapUtil>& map_util,
-            const planning_ros_msgs::VoxelMap& msg) {
+#include <algorithm>
+
+void setMap(const std::shared_ptr<MPL::VoxelMapUtil>& map_util,
+            const kr_planning_msgs::VoxelMap& msg) {
   Vec3f ori(msg.origin.x, msg.origin.y, msg.origin.z);
   Vec3i dim(msg.dim.x, msg.dim.y, msg.dim.z);
   map_util->setMap(ori, dim, msg.data, msg.resolution);
 }
 
-void getMap(std::shared_ptr<MPL::VoxelMapUtil>& map_util,
-            planning_ros_msgs::VoxelMap& map) {
+void getMap(const std::shared_ptr<MPL::VoxelMapUtil>& map_util,
+            kr_planning_msgs::VoxelMap* map) {
   Vec3f ori = map_util->getOrigin();
   Vec3i dim = map_util->getDim();
   double res = map_util->getRes();
 
-  map.origin.x = ori(0);
-  map.origin.y = ori(1);
-  map.origin.z = ori(2);
+  map->origin.x = ori(0);
+  map->origin.y = ori(1);
+  map->origin.z = ori(2);
 
-  map.dim.x = dim(0);
-  map.dim.y = dim(1);
-  map.dim.z = dim(2);
-  map.resolution = res;
+  map->dim.x = dim(0);
+  map->dim.y = dim(1);
+  map->dim.z = dim(2);
+  map->resolution = res;
 
-  map.data = map_util->getMap();
+  map->data = map_util->getMap();
 }
 
-void setMap(std::shared_ptr<JPS::VoxelMapUtil>& map_util,
-            const planning_ros_msgs::VoxelMap& msg) {
+void setMap(const std::shared_ptr<JPS::VoxelMapUtil>& map_util,
+            const kr_planning_msgs::VoxelMap& msg) {
   Vec3f ori(msg.origin.x, msg.origin.y, msg.origin.z);
   Vec3i dim(msg.dim.x, msg.dim.y, msg.dim.z);
   map_util->setMap(ori, dim, msg.data, msg.resolution);
 }
 
-void getMap(std::shared_ptr<JPS::VoxelMapUtil>& map_util,
-            planning_ros_msgs::VoxelMap& map) {
+void getMap(const std::shared_ptr<JPS::VoxelMapUtil>& map_util,
+            kr_planning_msgs::VoxelMap* map) {
   Vec3f ori = map_util->getOrigin();
   Vec3i dim = map_util->getDim();
   double res = map_util->getRes();
 
-  map.origin.x = ori(0);
-  map.origin.y = ori(1);
-  map.origin.z = ori(2);
+  map->origin.x = ori(0);
+  map->origin.y = ori(1);
+  map->origin.z = ori(2);
 
-  map.dim.x = dim(0);
-  map.dim.y = dim(1);
-  map.dim.z = dim(2);
-  map.resolution = res;
+  map->dim.x = dim(0);
+  map->dim.y = dim(1);
+  map->dim.z = dim(2);
+  map->resolution = res;
 
-  map.data = map_util->getMap();
+  map->data = map_util->getMap();
 }
 
-void setMap(std::shared_ptr<JPS::OccMapUtil>& map_util,
-            const planning_ros_msgs::VoxelMap& msg) {
+void setMap(const std::shared_ptr<JPS::OccMapUtil>& map_util,
+            const kr_planning_msgs::VoxelMap& msg) {
   Vec2f ori(msg.origin.x, msg.origin.y);
   Vec2i dim(msg.dim.x, msg.dim.y);
   map_util->setMap(ori, dim, msg.data, msg.resolution);
 }
 
-void getMap(std::shared_ptr<JPS::OccMapUtil>& map_util,
-            planning_ros_msgs::VoxelMap& map) {
+void getMap(const std::shared_ptr<JPS::OccMapUtil>& map_util,
+            kr_planning_msgs::VoxelMap* map) {
   Vec2f ori = map_util->getOrigin();
   Vec2i dim = map_util->getDim();
   double res = map_util->getRes();
 
-  map.origin.x = ori(0);
-  map.origin.y = ori(1);
-  map.origin.z = 0;
+  map->origin.x = ori(0);
+  map->origin.y = ori(1);
+  map->origin.z = 0;
 
-  map.dim.x = dim(0);
-  map.dim.y = dim(1);
-  map.dim.z = 1;
-  map.resolution = res;
+  map->dim.x = dim(0);
+  map->dim.y = dim(1);
+  map->dim.z = 1;
+  map->resolution = res;
 
-  map.data = map_util->getMap();
+  map->data = map_util->getMap();
 }
 
-planning_ros_msgs::VoxelMap sliceMap(const planning_ros_msgs::VoxelMap& map,
-                                     double h, double hh) {
+kr_planning_msgs::VoxelMap sliceMap(const kr_planning_msgs::VoxelMap& map,
+                                    double h,
+                                    double hh) {
   // slice a 3D voxel map
   double res = map.resolution;
   int hhi = hh / res;
@@ -88,7 +91,7 @@ planning_ros_msgs::VoxelMap sliceMap(const planning_ros_msgs::VoxelMap& map,
   h_max = h_max <= map.dim.z ? h_max : map.dim.z;
 
   // slice a 3D voxel map
-  planning_ros_msgs::VoxelMap voxel_map;
+  kr_planning_msgs::VoxelMap voxel_map;
   voxel_map.origin.x = map.origin.x;
   voxel_map.origin.y = map.origin.y;
   voxel_map.origin.z = h;

diff --git a/autonomy_core/map_plan/action_planner/src/data_conversions.h b/autonomy_core/map_plan/action_planner/src/data_conversions.h
@@ -1,28 +1,31 @@
 #pragma once
 
 #include <jps/map_util.h>
+#include <kr_planning_msgs/VoxelMap.h>
 #include <mpl_collision/map_util.h>
-#include <planning_ros_msgs/VoxelMap.h>
+#include <memory>
 
-void setMap(std::shared_ptr<MPL::VoxelMapUtil>& map_util,
-            const planning_ros_msgs::VoxelMap& msg);
 
-void getMap(std::shared_ptr<MPL::VoxelMapUtil>& map_util,
-            planning_ros_msgs::VoxelMap& map);
+void setMap(const std::shared_ptr<MPL::VoxelMapUtil>& map_util,
+            const kr_planning_msgs::VoxelMap& msg);
 
-void setMap(std::shared_ptr<JPS::VoxelMapUtil>& map_util,
-            const planning_ros_msgs::VoxelMap& msg);
+void getMap(const std::shared_ptr<MPL::VoxelMapUtil>& map_util,
+            kr_planning_msgs::VoxelMap* map);
 
-void getMap(std::shared_ptr<JPS::VoxelMapUtil>& map_util,
-            planning_ros_msgs::VoxelMap& map);
+void setMap(const std::shared_ptr<JPS::VoxelMapUtil>& map_util,
+            const kr_planning_msgs::VoxelMap& msg);
 
-void setMap(std::shared_ptr<JPS::OccMapUtil>& map_util,
-            const planning_ros_msgs::VoxelMap& msg);
+void getMap(const std::shared_ptr<JPS::VoxelMapUtil>& map_util,
+            kr_planning_msgs::VoxelMap* map);
 
-void getMap(std::shared_ptr<JPS::OccMapUtil>& map_util,
-            planning_ros_msgs::VoxelMap& map);
+void setMap(const std::shared_ptr<JPS::OccMapUtil>& map_util,
+            const kr_planning_msgs::VoxelMap& msg);
+
+void getMap(const std::shared_ptr<JPS::OccMapUtil>& map_util,
+            kr_planning_msgs::VoxelMap* map);
 
 // NOTE: This function is the same as getInflatedOccMap function in
 // voxel_mapper.cpp, should merge them.
-planning_ros_msgs::VoxelMap sliceMap(const planning_ros_msgs::VoxelMap& map,
-                                     double h, double hh = 0);
+kr_planning_msgs::VoxelMap sliceMap(const kr_planning_msgs::VoxelMap& map,
+                                    double h,
+                                    double hh = 0);