Update some packages to ROS2 Jazzy

autonomy_core/client/client_launch/CMakeLists.txt

@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.10)
 project(client_launch)
 
-find_package(catkin REQUIRED)
-catkin_package()
+find_package(ament_cmake REQUIRED)
+ament_package()

autonomy_core/client/client_launch/launch/client.launch

@@ -12,11 +12,11 @@
       <remap from="state_trigger" to="state_trigger"/>
       <remap from="motors" to="motors"/> -->
     </node>
-    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find client_launch)/rviz/client.rviz -f $(arg robot)/map" respawn="false">
+    <!-- <node pkg="rviz" type="rviz" name="rviz" args="-d $(find client_launch)/rviz/client.rviz -f $(arg robot)/map" respawn="false">
       <remap from="/$(arg viz_ns)/voxel_map" to="voxel_map"/>
       <remap from="/$(arg viz_ns)/trackers_manager/sg" to="trackers_manager/sg"/>
       <remap from="/$(arg viz_ns)/tpplanner/path_array" to="tpplanner/path_array"/>
       <remap from="/$(arg viz_ns)/odom" to="odom"/>
-    </node>
+    </node> -->
   </group>
 </launch>

autonomy_core/client/client_launch/package.xml

@@ -9,8 +9,9 @@
     <maintainer email="[email protected]">Chao Qu</maintainer>
 
     <license>Penn Software License</license>
-    <buildtool_depend>catkin</buildtool_depend>
+    <buildtool_depend>ament_cmake</buildtool_depend>
 
     <export>
+        <build_type>ament_cmake</build_type>
     </export>
 </package>

autonomy_core/map_plan/jps3d/CMakeLists.txt

@@ -3,14 +3,36 @@ project(jps3d)
 
 set(CMAKE_CXX_STANDARD 17)
 
-find_package(catkin REQUIRED
-            COMPONENTS kr_planning_msgs)
+find_package(ament_cmake)
+find_package(kr_planning_msgs REQUIRED)
 find_package(Eigen3 REQUIRED)
 find_package(Boost REQUIRED)
 
-catkin_package(INCLUDE_DIRS include LIBRARIES ${PROJECT_NAME} CATKIN_DEPENDS kr_planning_msgs)
-
 add_library(${PROJECT_NAME} src/graph_search.cpp src/jps_planner.cpp
                             src/map_util.cpp)
-target_include_directories(${PROJECT_NAME} PUBLIC include ${catkin_INCLUDE_DIRS})
-target_link_libraries(${PROJECT_NAME} PUBLIC Eigen3::Eigen Boost::boost)
+target_include_directories(${PROJECT_NAME} PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include/${PROJECT_NAME}>)
+target_link_libraries(${PROJECT_NAME} Eigen3::Eigen Boost::boost)
+
+ament_target_dependencies(${PROJECT_NAME} kr_planning_msgs)
+
+
+# Install targets
+install(TARGETS ${PROJECT_NAME}
+  EXPORT export_${PROJECT_NAME}
+  LIBRARY DESTINATION lib/${PROJECT_NAME}
+)
+
+install(DIRECTORY include/
+  DESTINATION include/${PROJECT_NAME}
+)
+
+# Export targets and declare package
+ament_export_targets(export_${PROJECT_NAME})
+ament_export_dependencies(
+  kr_planning_msgs
+  Eigen3
+  Boost
+)
+ament_package()

autonomy_core/map_plan/jps3d/include/jps/map_util.h

@@ -5,7 +5,7 @@
 #pragma once
 
 #include "jps/data_type.h"
-#include <kr_planning_msgs/VoxelMap.h>
+#include <kr_planning_msgs/msg/voxel_map.hpp>
 
 namespace JPS {
 
@@ -101,7 +101,7 @@ class MapUtil {
   /// Map entity
   Tmap map_;
 
-  kr_planning_msgs::VoxelMap voxel_map;
+  kr_planning_msgs::msg::VoxelMap voxel_map;
 
  protected:
   /// Resolution
@@ -112,13 +112,13 @@ class MapUtil {
   Veci<Dim> dim_;
   /// Assume occupied cell has value 100
   // Now replaced with parameter value from VoxelMap.msg
-  int8_t val_occ = voxel_map.val_occ;
+  int8_t val_occ = voxel_map.VAL_OCC;
   /// Assume free cell has value 0
   // Now replaced with parameter value from VoxelMap.msg
-  int8_t val_free = voxel_map.val_free;
+  int8_t val_free = voxel_map.VAL_FREE;
   /// Assume unknown cell has value -1
   // Now replaced with parameter value from VoxelMap.msg
-  int8_t val_unknown = voxel_map.val_unknown;
+  int8_t val_unknown = voxel_map.VAL_UNKNOWN;
 };
 
 typedef MapUtil<2> OccMapUtil;

autonomy_core/map_plan/jps3d/package.xml

@@ -4,13 +4,15 @@
   <version>1.0.0</version>
   <description>The jps3d package</description>
   <maintainer email="[email protected]">sikang</maintainer>
+  <maintainer email="[email protected]">Jiuzhou Lei</maintainer>
 
   <license>BSD-3-Clause</license>
-  <buildtool_depend>catkin</buildtool_depend>
+  <buildtool_depend>ament_cmake</buildtool_depend>
 
   <depend>kr_planning_msgs</depend>
 
   <export>
+    <build_type>ament_cmake</build_type>
   </export>
 
 </package>

autonomy_core/map_plan/map_plan_launch/CMakeLists.txt

@@ -1,5 +1,16 @@
 cmake_minimum_required(VERSION 3.10)
 project(map_plan_launch)
 
-find_package(catkin REQUIRED)
-catkin_package()
+# find_package(catkin REQUIRED)
+# catkin_package()
+find_package(ament_cmake REQUIRED)
+
+install(
+  DIRECTORY launch
+  DESTINATION share/${PROJECT_NAME}
+)
+install(
+  DIRECTORY config
+  DESTINATION share/${PROJECT_NAME}
+)
+ament_package()

autonomy_core/map_plan/map_plan_launch/config/mapper.yaml

@@ -1,29 +1,31 @@
-robot_r: 0.50 # robot radius, local and global maps will be dilated in x and y with a radius of [ceil(robot_r/res)] cells
-robot_h: 0.25 # robot height, local and global maps will be dilated in z direction for 2*[ceil(robot_h/res)] cells (if robot_r is set as 0, there will not be any dilation along z either).
+/mapper:
+  ros__parameters:
+    robot_r: 0.50 # robot radius, local and global maps will be dilated in x and y with a radius of [ceil(robot_r/res)] cells
+    robot_h: 0.25 # robot height, local and global maps will be dilated in z direction for 2*[ceil(robot_h/res)] cells (if robot_r is set as 0, there will not be any dilation along z either).
 
-# global mapper config
-global:
-  center_x: 0.0 # center of the map in x-axis 
-  center_y: 0.0 # center of the map in y-axis
-  range_x: 200.0 # range of the map in x-axis, map will span over [center_x - 0.5*range_x, center_x + 0.5*range_x] (high impact on memory and computation usage, recommended value: 500)
-  range_y: 200.0 # range of the map in y-axis, map will span over [center_y - 0.5*range_y, center_y + 0.5*range_y] (high impact on memory and computation usage, recommended value: 500
-  center_z: 5.0 # center of the map in z-axis, should be similar to the robot take_off_height
-  range_z: 10.0 # range of the map in z-axis, map will span over [center_z - 0.5*range_z, center_z + 0.5*range_z] (high impact on memory and computation usage, recommended value: 10)
-  resolution: 0.5 # resolution of the map (high impact on memory and computation usage, recommended value: 1.0~2.0 for sparse environments, 0.5 for dense environments) 
-  num_point_cloud_skip: 3 # the global map update frequency = point cloud frequency / num_point_cloud_skip
-  max_raycast_range: 30.0 # max raycast range, the smaller, the less computation demand, but shorter perception range
-  dilate_xy: true # dilate global map along x and y axis (recommended value: true)
-  dilate_z: false # dilate global map along z axis (recommended value: false)
-  decay_times_to_empty: 30 # number of times of decay to empty, 0 means no decay, the noisier your state estimator is, the smaller this value should be (when using ground-truth pose, set this as 0)
+    # global mapper config
+    global:
+      center_x: 0.0 # center of the map in x-axis 
+      center_y: 0.0 # center of the map in y-axis
+      range_x: 200.0 # range of the map in x-axis, map will span over [center_x - 0.5*range_x, center_x + 0.5*range_x] (high impact on memory and computation usage, recommended value: 500)
+      range_y: 200.0 # range of the map in y-axis, map will span over [center_y - 0.5*range_y, center_y + 0.5*range_y] (high impact on memory and computation usage, recommended value: 500
+      center_z: 5.0 # center of the map in z-axis, should be similar to the robot take_off_height
+      range_z: 10.0 # range of the map in z-axis, map will span over [center_z - 0.5*range_z, center_z + 0.5*range_z] (high impact on memory and computation usage, recommended value: 10)
+      resolution: 0.5 # resolution of the map (high impact on memory and computation usage, recommended value: 1.0~2.0 for sparse environments, 0.5 for dense environments) 
+      num_point_cloud_skip: 3 # the global map update frequency = point cloud frequency / num_point_cloud_skip
+      max_raycast_range: 30.0 # max raycast range, the smaller, the less computation demand, but shorter perception range
+      dilate_xy: true # dilate global map along x and y axis (recommended value: true)
+      dilate_z: false # dilate global map along z axis (recommended value: false)
+      decay_times_to_empty: 30 # number of times of decay to empty, 0 means no decay, the noisier your state estimator is, the smaller this value should be (when using ground-truth pose, set this as 0)
 
-local: # local map range z and center_z will be the same as storage map
-  # local planning horizon will be from robot to the local voxel map boundary
-  # center of local map will be the same as robot current odometry along x and y direction, and the same as global map along z direction
-  range_x: 25.0 # range of the map in x-axis (high impact on memory and computation usage, recommended value: 40)
-  range_y: 25.0 # range of the map in x-axis (high impact on memory and computation usage, recommended value: 40)
-  range_z: 12.0 # range of the map in z-axis (recommended value: slightly larger than global map range z to handle overshoot along z-axis upon stopping policy) 
-  resolution: 0.25 # resolution of the map (high impact on memory and computation usage, recommended value: 0.5~1.0 for sparse environments, 0.1~0.5 for dense environments) 
-  max_raycast_range: 13.0 # max raycast range, the smaller, the less computation demand, but shorter perception range
-  decay_times_to_empty: 30 # number of times of decay to empty, 0 means no decay, the noisier your state estimator is, the smaller this value should be (when using ground-truth pose, set this as 0)
+    local: # local map range z and center_z will be the same as storage map
+      # local planning horizon will be from robot to the local voxel map boundary
+      # center of local map will be the same as robot current odometry along x and y direction, and the same as global map along z direction
+      range_x: 25.0 # range of the map in x-axis (high impact on memory and computation usage, recommended value: 40)
+      range_y: 25.0 # range of the map in x-axis (high impact on memory and computation usage, recommended value: 40)
+      range_z: 12.0 # range of the map in z-axis (recommended value: slightly larger than global map range z to handle overshoot along z-axis upon stopping policy) 
+      resolution: 0.25 # resolution of the map (high impact on memory and computation usage, recommended value: 0.5~1.0 for sparse environments, 0.1~0.5 for dense environments) 
+      max_raycast_range: 13.0 # max raycast range, the smaller, the less computation demand, but shorter perception range
+      decay_times_to_empty: 30 # number of times of decay to empty, 0 means no decay, the noisier your state estimator is, the smaller this value should be (when using ground-truth pose, set this as 0)
 
 

autonomy_core/map_plan/map_plan_launch/launch/depth_to_cloud.launch.py

@@ -0,0 +1,48 @@
+from launch import LaunchDescription
+from launch_ros.actions import ComposableNodeContainer
+from launch_ros.descriptions import ComposableNode
+from launch_ros.actions import LoadComposableNodes
+from launch.substitutions import LaunchConfiguration
+
+def generate_launch_description():
+    # Arguments
+    image_raw = LaunchConfiguration('image_raw', default='image_raw')
+    points = LaunchConfiguration('points', default='points')
+
+    # Create a component container
+    container = ComposableNodeContainer(
+        name='depth_image_container',
+        namespace='',
+        package='rclcpp_components',
+        executable='component_container',
+        output='screen',
+    )
+
+    # Metric conversion component
+    metric_rect = ComposableNode(
+        package='depth_image_proc',
+        plugin='depth_image_proc::ConvertMetricNode',
+        name='metric_rect',
+        remappings=[
+            ('image_raw', image_raw),
+            ('image', 'image_rect'),
+        ],
+    )
+
+    # Point cloud conversion component
+    cloudify = ComposableNode(
+        package='depth_image_proc',
+        plugin='depth_image_proc::PointCloudXyzNode',
+        name='cloudify',
+        remappings=[
+            ('points', points),
+        ],
+    )
+
+    return LaunchDescription([
+        container,
+        LoadComposableNodes(
+            target_container=container,
+            composable_node_descriptions=[metric_rect, cloudify],
+        )
+    ])

autonomy_core/map_plan/map_plan_launch/launch/map_plan.launch.xml

@@ -0,0 +1,39 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<launch>
+  <arg name="robot" default="$(env ROBOT_NAME 'quadrotor')"/>
+  <arg name="real_robot" default="false"/>
+  <arg name="use_mp" default="false"/>
+  <arg name="min_dispersion_planner" default="false"/>
+  <arg name="odom" default="odom"/>
+
+  <!-- lidar_frame arg is only used when real_robot is true, otherwise the mapper will just use the same frame as the point cloud in simulator -->
+  <arg name="lidar_frame" default=""/>
+  <arg name="mapper_config" default="$(find-pkg-share map_plan_launch)/config/mapper.yaml"/>
+  <arg name="planner_config" default="$(find-pkg-share control_launch)/config/tracker_params_mp.yaml"/>
+  <arg name="cloud" default="lidar"/>
+
+  <group>
+    <push-ros-namespace namespace="$(var robot)"/>
+
+    <include file="$(find-pkg-share map_plan_launch)/launch/mapper.launch.xml">
+      <arg name="odom_frame" value="$(var robot)/odom"/>
+      <arg name="map_frame" value="$(var robot)/map"/>
+      <arg name="lidar_frame" value="$(var lidar_frame)"/>
+      <arg name="real_robot" value="$(var real_robot)"/>
+      <arg name="mapper_config" value="$(var mapper_config)"/>
+      <arg name="cloud" value="$(var cloud)"/>
+    </include>
+
+    <group unless="$(var use_mp)">
+      <include file="$(find-pkg-share map_plan_launch)/launch/planner.launch.xml"/>
+    </group>
+
+    <group if="$(var use_mp)">
+      <include file="$(find-pkg-share map_plan_launch)/launch/mp_planner.launch.xml">
+        <arg name="planner_config" value="$(var planner_config)"/>
+        <arg name="odom" value="$(var odom)"/>
+        <arg name="min_dispersion_planner" value="$(var min_dispersion_planner)"/>
+      </include>
+    </group>
+  </group>
+</launch>

autonomy_core/map_plan/map_plan_launch/launch/mapper.launch.xml

@@ -0,0 +1,27 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<launch>
+  <arg name="cloud" default="lidar"/>
+  <arg name="odom_frame" default="odom"/>
+  <arg name="map_frame" default="map"/>
+  <arg name="lidar_frame" default=""/>
+  <arg name="real_robot" default="false"/>
+  <arg name="mapper_config" default="$(find-pkg-share map_plan_launch)/config/mapper.yaml"/>
+  <arg name="prefix" default="nice -n 1"/>
+
+  <node 
+    pkg="mapper" 
+    name="mapper" 
+    exec="local_global_mapper" 
+    output="screen" 
+    launch-prefix="$(var prefix)">
+    <param name="real_robot" value="$(var real_robot)"/>
+    <param name="map_frame" value="$(var map_frame)"/>
+    <param name="odom_frame" value="$(var odom_frame)"/>
+    <param name="lidar_frame" value="$(var lidar_frame)"/>
+    <param from="$(var mapper_config)"/>
+    <remap from="~/cloud" to="$(var cloud)"/>
+    <remap from="~/voxel_map" to="voxel_map"/>
+    <remap from="~/local_cloud" to="local_cloud"/>
+
+  </node>
+</launch>

autonomy_core/map_plan/map_plan_launch/launch/mp_planner.launch.xml

@@ -0,0 +1,32 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<launch>
+  <arg name="prefix" default="nice -n 2"/>
+  <arg name="odom" default="odom"/>
+  <arg name="min_dispersion_planner" default="false"/>
+  <arg name="planner_config" default="$(find-pkg-share control_launch)/config/tracker_params_mp.yaml"/>
+
+  <node 
+    pkg="action_planner"
+    exec="global_plan_server"
+    name="global_plan_server"
+    output="screen"
+    launch-prefix="$(var prefix)">
+    <param from="$(var planner_config)"/>
+    <remap from="~/odom" to="$(var odom)"/>
+    <remap from="~/global_voxel_map" to="mapper/global_voxel_map"/>
+  </node>
+
+  <!-- If NOT use minimum dispersion motion primitive planner -->
+  <group unless="$(var min_dispersion_planner)">
+    <node 
+      pkg="action_planner"
+      exec="local_plan_server"
+      name="local_plan_server"
+      output="screen"
+      launch-prefix="$(var prefix)">
+
+      <param from="$(var planner_config)"/>
+      <remap from="~/local_voxel_map" to="mapper/local_voxel_map"/>
+    </node>
+  </group>  
+</launch>

autonomy_core/map_plan/map_plan_launch/launch/planner.launch.xml

@@ -0,0 +1,8 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<launch>
+  <!-- use nice to set low priority for planner -->
+  <node pkg="action_planner" exec="path_planner" name="tpplanner" clear_params="true" output="screen" launch-prefix="nice -n 1">
+    <remap from="~voxel_map" to="voxel_map"/>
+    <param name="verbose" value="true"/>
+  </node>
+</launch>

autonomy_core/map_plan/map_plan_launch/package.xml

@@ -7,10 +7,12 @@
     <maintainer email="[email protected]">Xu Liu</maintainer>
     <maintainer email="[email protected]">Chao Qu</maintainer>
     <maintainer email="[email protected]">Guilherme Nardari</maintainer>
+    <maintainer email="[email protected]">Jiuzhou Lei</maintainer>
 
     <license>Penn Software License</license>
-    <buildtool_depend>catkin</buildtool_depend>
+    <buildtool_depend>ament_cmake</buildtool_depend>
 
     <export>
+        <build_type>ament_cmake</build_type>
     </export>
 </package>