diff --git a/autonomy_core/client/client_launch/CMakeLists.txt b/autonomy_core/client/client_launch/CMakeLists.txt index eb5554f8..236a2bbe 100644 --- a/autonomy_core/client/client_launch/CMakeLists.txt +++ b/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() diff --git a/autonomy_core/client/client_launch/launch/client.launch b/autonomy_core/client/client_launch/launch/client.launch index dc5b52bc..40d5a88f 100644 --- a/autonomy_core/client/client_launch/launch/client.launch +++ b/autonomy_core/client/client_launch/launch/client.launch @@ -12,11 +12,11 @@ --> - + diff --git a/autonomy_core/client/client_launch/package.xml b/autonomy_core/client/client_launch/package.xml index f2ab02e6..82f6a9de 100644 --- a/autonomy_core/client/client_launch/package.xml +++ b/autonomy_core/client/client_launch/package.xml @@ -9,8 +9,9 @@ Chao Qu Penn Software License - catkin + ament_cmake + ament_cmake diff --git a/autonomy_core/map_plan/jps3d/CMakeLists.txt b/autonomy_core/map_plan/jps3d/CMakeLists.txt index 17ac011d..03ddcb95 100644 --- a/autonomy_core/map_plan/jps3d/CMakeLists.txt +++ b/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 + $ + $) +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() \ No newline at end of file diff --git a/autonomy_core/map_plan/jps3d/include/jps/map_util.h b/autonomy_core/map_plan/jps3d/include/jps/map_util.h index 760af5a1..4b1e6acf 100644 --- a/autonomy_core/map_plan/jps3d/include/jps/map_util.h +++ b/autonomy_core/map_plan/jps3d/include/jps/map_util.h @@ -5,7 +5,7 @@ #pragma once #include "jps/data_type.h" -#include +#include 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_; /// 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; diff --git a/autonomy_core/map_plan/jps3d/package.xml b/autonomy_core/map_plan/jps3d/package.xml index a0d59db0..6210a3a9 100644 --- a/autonomy_core/map_plan/jps3d/package.xml +++ b/autonomy_core/map_plan/jps3d/package.xml @@ -4,13 +4,15 @@ 1.0.0 The jps3d package sikang + Jiuzhou Lei BSD-3-Clause - catkin + ament_cmake kr_planning_msgs + ament_cmake diff --git a/autonomy_core/map_plan/map_plan_launch/CMakeLists.txt b/autonomy_core/map_plan/map_plan_launch/CMakeLists.txt index c7081b2f..125836b1 100644 --- a/autonomy_core/map_plan/map_plan_launch/CMakeLists.txt +++ b/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() diff --git a/autonomy_core/map_plan/map_plan_launch/config/mapper.yaml b/autonomy_core/map_plan/map_plan_launch/config/mapper.yaml index 8428b527..481fabf2 100644 --- a/autonomy_core/map_plan/map_plan_launch/config/mapper.yaml +++ b/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) diff --git a/autonomy_core/map_plan/map_plan_launch/launch/depth_to_cloud.launch.py b/autonomy_core/map_plan/map_plan_launch/launch/depth_to_cloud.launch.py new file mode 100644 index 00000000..d68642bc --- /dev/null +++ b/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], + ) + ]) \ No newline at end of file diff --git a/autonomy_core/map_plan/map_plan_launch/launch/map_plan.launch.xml b/autonomy_core/map_plan/map_plan_launch/launch/map_plan.launch.xml new file mode 100644 index 00000000..c42520a2 --- /dev/null +++ b/autonomy_core/map_plan/map_plan_launch/launch/map_plan.launch.xml @@ -0,0 +1,39 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/autonomy_core/map_plan/map_plan_launch/launch/mapper.launch.xml b/autonomy_core/map_plan/map_plan_launch/launch/mapper.launch.xml new file mode 100644 index 00000000..4e538aac --- /dev/null +++ b/autonomy_core/map_plan/map_plan_launch/launch/mapper.launch.xml @@ -0,0 +1,27 @@ + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/autonomy_core/map_plan/map_plan_launch/launch/mp_planner.launch.xml b/autonomy_core/map_plan/map_plan_launch/launch/mp_planner.launch.xml new file mode 100644 index 00000000..ac56a14b --- /dev/null +++ b/autonomy_core/map_plan/map_plan_launch/launch/mp_planner.launch.xml @@ -0,0 +1,32 @@ + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/autonomy_core/map_plan/map_plan_launch/launch/planner.launch.xml b/autonomy_core/map_plan/map_plan_launch/launch/planner.launch.xml new file mode 100644 index 00000000..0af426fd --- /dev/null +++ b/autonomy_core/map_plan/map_plan_launch/launch/planner.launch.xml @@ -0,0 +1,8 @@ + + + + + + + + \ No newline at end of file diff --git a/autonomy_core/map_plan/map_plan_launch/package.xml b/autonomy_core/map_plan/map_plan_launch/package.xml index 3d528846..cf5a0f5e 100644 --- a/autonomy_core/map_plan/map_plan_launch/package.xml +++ b/autonomy_core/map_plan/map_plan_launch/package.xml @@ -7,10 +7,12 @@ Xu Liu Chao Qu Guilherme Nardari + Jiuzhou Lei Penn Software License - catkin + ament_cmake + ament_cmake diff --git a/autonomy_core/map_plan/mapper/CMakeLists.txt b/autonomy_core/map_plan/mapper/CMakeLists.txt index 4fed1af9..ff0c0359 100644 --- a/autonomy_core/map_plan/mapper/CMakeLists.txt +++ b/autonomy_core/map_plan/mapper/CMakeLists.txt @@ -1,42 +1,42 @@ -cmake_minimum_required(VERSION 3.10) -project(mapper) +# cmake_minimum_required(VERSION 3.10) +# project(mapper) -set(CMAKE_CXX_STANDARD 17) -find_package( - catkin REQUIRED - COMPONENTS roscpp - pcl_ros - image_geometry - kr_planning_msgs - kr_planning_rviz_plugins - depth_image_proc - sensor_msgs - eigen_conversions - tf_conversions - motion_primitive_library) +# set(CMAKE_CXX_STANDARD 17) +# find_package( +# catkin REQUIRED +# COMPONENTS roscpp +# pcl_ros +# image_geometry +# kr_planning_msgs +# kr_planning_rviz_plugins +# depth_image_proc +# sensor_msgs +# eigen_conversions +# tf_conversions +# motion_primitive_library) -find_package(Boost REQUIRED COMPONENTS timer) +# find_package(Boost REQUIRED COMPONENTS timer) -catkin_package(INCLUDE_DIRS include LIBRARIES ${PROJECT_NAME}) +# catkin_package(INCLUDE_DIRS include LIBRARIES ${PROJECT_NAME}) -include_directories(${PROJECT_NAME} PUBLIC ${catkin_INCLUDE_DIRS} include) +# include_directories(${PROJECT_NAME} PUBLIC ${catkin_INCLUDE_DIRS} include) -add_library(${PROJECT_NAME} src/tf_listener.cpp src/voxel_mapper.cpp) -target_link_libraries(${PROJECT_NAME} PUBLIC ${catkin_LIBRARIES}) +# add_library(${PROJECT_NAME} src/tf_listener.cpp src/voxel_mapper.cpp) +# target_link_libraries(${PROJECT_NAME} PUBLIC ${catkin_LIBRARIES}) -add_executable(local_global_mapper src/local_global_mapper.cpp) -target_link_libraries(local_global_mapper PUBLIC ${PROJECT_NAME} Boost::timer) +# add_executable(local_global_mapper src/local_global_mapper.cpp) +# target_link_libraries(local_global_mapper PUBLIC ${PROJECT_NAME} Boost::timer) -#---------------------------------------------------------------------------------- -# Add testing with GoogleTest +# #---------------------------------------------------------------------------------- +# # Add testing with GoogleTest -# To run test: -# catkin test mapper -enable_testing() +# # To run test: +# # catkin test mapper +# enable_testing() -catkin_add_gtest(test_voxel_mapper test/test_voxel_mapper.cpp) -target_link_libraries(test_voxel_mapper ${catkin_LIBRARIES} ${PROJECT_NAME}) +# catkin_add_gtest(test_voxel_mapper test/test_voxel_mapper.cpp) +# target_link_libraries(test_voxel_mapper ${catkin_LIBRARIES} ${PROJECT_NAME}) #---------------------------------------------------------------------------------- # Add microbenchmarking with Google Benchmark @@ -62,3 +62,117 @@ target_link_libraries(test_voxel_mapper ${catkin_LIBRARIES} ${PROJECT_NAME}) # target_link_libraries(benchmarking benchmark::benchmark ${PROJECT_NAME}) +cmake_minimum_required(VERSION 3.10) +project(mapper) + +# Use C++17 +set(CMAKE_CXX_STANDARD 17) + +# ----------------------------------------------------------------------------- +# 1) ament & ROS 2 packages instead of catkin +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(pcl_ros REQUIRED) +find_package(image_geometry REQUIRED) +find_package(kr_planning_msgs REQUIRED) +find_package(kr_planning_rviz_plugins REQUIRED) +find_package(depth_image_proc REQUIRED) +find_package(sensor_msgs REQUIRED) +# find_package(eigen_conversions REQUIRED) # Eigen conversions for ROS 2 is not supported yet +find_package(tf2_eigen REQUIRED) +find_package(motion_primitive_library REQUIRED) +find_package(Boost REQUIRED COMPONENTS timer) + +# ----------------------------------------------------------------------------- +# 2) Build your library +add_library(${PROJECT_NAME} + src/tf_listener.cpp + src/voxel_mapper.cpp +) +# Declare dependencies for include dirs and link targets +ament_target_dependencies(${PROJECT_NAME} + rclcpp + pcl_ros + image_geometry + kr_planning_msgs + kr_planning_rviz_plugins + depth_image_proc + sensor_msgs + # eigen_conversions + tf2_eigen + motion_primitive_library +) +# Link the Boost::timer target +target_link_libraries(${PROJECT_NAME} Boost::timer) +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $ +) + +# ----------------------------------------------------------------------------- +# 3) Build your node +add_executable(local_global_mapper src/local_global_mapper.cpp) +target_link_libraries(local_global_mapper PUBLIC ${PROJECT_NAME} Boost::timer) +target_include_directories(local_global_mapper PUBLIC + $ + $ +) + +# ----------------------------------------------------------------------------- +# 4) Testing with ament_cmake_gtest +# if(BUILD_TESTING) +# find_package(ament_lint_auto REQUIRED) +# ament_lint_auto_find_test_dependencies() + +# ament_add_gtest(test_voxel_mapper test/test_voxel_mapper.cpp) +# if(TARGET test_voxel_mapper) +# ament_target_dependencies(test_voxel_mapper +# rclcpp +# pcl_ros +# image_geometry +# kr_planning_msgs +# kr_planning_rviz_plugins +# depth_image_proc +# sensor_msgs +# # eigen_conversions +# tf2_eigen +# motion_primitive_library +# ) +# target_link_libraries(test_voxel_mapper PUBLIC ${PROJECT_NAME} Boost::timer) +# endif() +# endif() + +# ----------------------------------------------------------------------------- +# 5) Install rules +install( + TARGETS + ${PROJECT_NAME} + LIBRARY DESTINATION lib/${PROJECT_NAME} +) +install( + TARGETS + local_global_mapper + RUNTIME DESTINATION lib/${PROJECT_NAME} +) + +install( + DIRECTORY include/ + DESTINATION include/${PROJECT_NAME} +) + +# ----------------------------------------------------------------------------- +# 6) Export & finalize +ament_export_dependencies( + rclcpp + pcl_ros + image_geometry + kr_planning_msgs + kr_planning_rviz_plugins + depth_image_proc + sensor_msgs + # eigen_conversions + tf2_eigen + motion_primitive_library + Boost +) +ament_package() diff --git a/autonomy_core/map_plan/mapper/changelog.md b/autonomy_core/map_plan/mapper/changelog.md new file mode 100644 index 00000000..c8438a97 --- /dev/null +++ b/autonomy_core/map_plan/mapper/changelog.md @@ -0,0 +1,15 @@ +# Changelog + +## [Unreleased] +- Initial creation of `changelog.md` to track changes in the mapper module. + +## Files Changed +- CMakelists.txt: use ament_cmake as build tools +- package.xml +- src/ +- include/ + +## Note +sensor_msgs::msg::pointcloud is still used in the code, suggested to migrate to pointcloud2 in the future + +Test relevant files are not updated in either the cpp file or compiled in CMAKELISTS.TXT \ No newline at end of file diff --git a/autonomy_core/map_plan/mapper/include/mapper/local_global_mapper.h b/autonomy_core/map_plan/mapper/include/mapper/local_global_mapper.h index 423ed1a9..ed36c621 100644 --- a/autonomy_core/map_plan/mapper/include/mapper/local_global_mapper.h +++ b/autonomy_core/map_plan/mapper/include/mapper/local_global_mapper.h @@ -1,10 +1,18 @@ -#include -#include +// #include +// #include +#include #include -#include -#include -#include -#include +// #include +#include +// #include +// #include +// #include +#include +#include +#include +#include +#include +#include #include #include @@ -17,14 +25,14 @@ using boost::timer::cpu_timer; using boost::timer::cpu_times; -class LocalGlobalMapperNode { +class LocalGlobalMapperNode : public rclcpp::Node { public: /** * @brief Local Global Mapper Constructor - * @param nh ROS Node handler + * @param options Node options for rclcpp::Node */ - explicit LocalGlobalMapperNode(const ros::NodeHandle& nh); - + // explicit LocalGlobalMapperNode(const ros::NodeHandle& nh); + explicit LocalGlobalMapperNode(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); private: /** * @brief Reads parameters from ROS parameter server @@ -49,23 +57,28 @@ class LocalGlobalMapperNode { * @param pose_map_lidar_ptr Output tf from lidar to map * @param pose_odom_lidar_ptr Output tf from lidar to odom */ - void getLidarPoses(const std_msgs::Header& cloud_header, - geometry_msgs::Pose* pose_map_lidar_ptr, - geometry_msgs::Pose* pose_odom_lidar_ptr); + // void getLidarPoses(const std_msgs::Header& cloud_header, + // geometry_msgs::msg::Pose* pose_map_lidar_ptr, + // geometry_msgs::msg::Pose* pose_odom_lidar_ptr); + void getLidarPoses(const std_msgs::msg::Header& cloud_header, + geometry_msgs::msg::Pose* pose_map_lidar_ptr, + geometry_msgs::msg::Pose* pose_odom_lidar_ptr); /** * @brief Adds input cloud to storage map, publishes new local map and global * map (if enough msgs were received) * @param cloud Input cloud message */ - void processCloud(const sensor_msgs::PointCloud& cloud); + // void processCloud(const sensor_msgs::PointCloud& cloud); + void processCloud(const sensor_msgs::msg::PointCloud& cloud); /** * @brief Point Cloud topic callback. Will convert from * sensor_msgs::PointCloud2 to sensor_msgs::PointCloud * @param msg Const pointer to input cloud message */ - void cloudCallback(const sensor_msgs::PointCloud2::ConstPtr& msg); + // void cloudCallback(const sensor_msgs::PointCloud2::ConstPtr& msg); + void cloudCallback(const sensor_msgs::msg::PointCloud2::SharedPtr msg); /** * @brief Allocates memory for the global mapper and initializes the arrays @@ -86,24 +99,29 @@ class LocalGlobalMapperNode { cpu_timer timer; // Timing stuff - ros::Publisher time_pub; + // ros::Publisher time_pub; + rclcpp::Publisher::SharedPtr time_pub; std::unique_ptr global_voxel_mapper_; // mapper std::unique_ptr storage_voxel_mapper_; // mapper // std::unique_ptr local_voxel_mapper_; // mapper - kr_planning_msgs::VoxelMap global_map_info_; - kr_planning_msgs::VoxelMap storage_map_info_; - kr_planning_msgs::VoxelMap local_map_info_; + kr_planning_msgs::msg::VoxelMap global_map_info_; + kr_planning_msgs::msg::VoxelMap storage_map_info_; + kr_planning_msgs::msg::VoxelMap local_map_info_; - ros::NodeHandle nh_; - ros::Subscriber cloud_sub; - ros::Publisher global_map_pub; - ros::Publisher storage_map_pub; - ros::Publisher local_map_pub; + // ros::NodeHandle nh_; + // ros::Subscriber cloud_sub; + // ros::Publisher global_map_pub; + // ros::Publisher storage_map_pub; + // ros::Publisher local_map_pub; + rclcpp::Subscription::SharedPtr cloud_sub; + rclcpp::Publisher::SharedPtr global_map_pub; + rclcpp::Publisher::SharedPtr storage_map_pub; + rclcpp::Publisher::SharedPtr local_map_pub; // ros::Publisher global_occ_map_pub; - ros::Publisher local_voxel_map_pub; + // ros::Publisher local_voxel_map_pub; // ros::Publisher local_cloud_pub; bool real_robot_; // define it's real-robot experiment or not @@ -133,8 +151,7 @@ class LocalGlobalMapperNode { double local_max_raycast_, global_max_raycast_; // maximum raycasting range double occ_map_height_; Eigen::Vector3d local_ori_offset_; - bool pub_storage_map_ = - false; // don't set this as true unless you're debugging, it's very slow + bool pub_storage_map_ = false; // don't set this as true unless you're debugging, it's very slow int update_interval_; int counter_ = 0; diff --git a/autonomy_core/map_plan/mapper/include/mapper/tf_listener.h b/autonomy_core/map_plan/mapper/include/mapper/tf_listener.h index 95479d12..67f10d89 100644 --- a/autonomy_core/map_plan/mapper/include/mapper/tf_listener.h +++ b/autonomy_core/map_plan/mapper/include/mapper/tf_listener.h @@ -1,24 +1,36 @@ #pragma once -#include -#include +// #include +// #include +// #include +#include +#include +#include #include #include +#include namespace mapper { class TFListener { public: - TFListener() : listener_(buffer_) {} + TFListener(rclcpp::Node::SharedPtr node) : node_(node), + buffer_(std::make_shared(node_->get_clock())), + listener_(std::make_shared(*buffer_)) {} + + // std::optional LookupTransform(const std::string& target, + // const std::string& source, + // const ros::Time& time); + std::optional LookupTransform( + const std::string& target, const std::string& source, + const rclcpp::Time& time); - std::optional LookupTransform(const std::string& target, - const std::string& source, - const ros::Time& time); private: - tf2_ros::Buffer buffer_; - tf2_ros::TransformListener listener_; + std::shared_ptr buffer_; + std::shared_ptr listener_; + rclcpp::Node::SharedPtr node_; }; } // namespace mapper diff --git a/autonomy_core/map_plan/mapper/include/mapper/voxel_mapper.h b/autonomy_core/map_plan/mapper/include/mapper/voxel_mapper.h index 0a1e9a4d..eeaf2b72 100644 --- a/autonomy_core/map_plan/mapper/include/mapper/voxel_mapper.h +++ b/autonomy_core/map_plan/mapper/include/mapper/voxel_mapper.h @@ -1,12 +1,13 @@ #pragma once -#include +#include #include #include #include #include #include +#include namespace mapper { @@ -51,6 +52,7 @@ class VoxelMapper { VoxelMapper(const Eigen::Vector3d& origin, const Eigen::Vector3d& dim, double res, + const rclcpp::Logger& logger, int8_t default_val = 0, int decay_times_to_empty = 0); @@ -86,22 +88,22 @@ class VoxelMapper { /** * @brief Get the Map object - * @return kr_planning_msgs::VoxelMap + * @return kr_planning_msgs::msg::VoxelMap */ - kr_planning_msgs::VoxelMap getMap(); + kr_planning_msgs::msg::VoxelMap getMap(); /** * @brief Get the Inflated Map object - * @return kr_planning_msgs::VoxelMap + * @return kr_planning_msgs::msg::VoxelMap */ - kr_planning_msgs::VoxelMap getInflatedMap(); + kr_planning_msgs::msg::VoxelMap getInflatedMap(); /** * @brief Crop a local voxel map from the global inflated voxel map * @param ori The origin of the local voxel map (most negative corner) * @param dim the range of the local voxel map in world units */ - kr_planning_msgs::VoxelMap getInflatedLocalMap(const Eigen::Vector3d& ori, + kr_planning_msgs::msg::VoxelMap getInflatedLocalMap(const Eigen::Vector3d& ori, const Eigen::Vector3d& dim); /** @@ -111,7 +113,7 @@ class VoxelMapper { * refers to how much space (in either direction) along the z-axis is going to * be considered for the 2D slice. */ - kr_planning_msgs::VoxelMap getInflatedOccMap(double h, double hh = 0); + kr_planning_msgs::msg::VoxelMap getInflatedOccMap(double h, double hh = 0); /** * @brief Add point cloud to map and inflated map @@ -204,16 +206,16 @@ class VoxelMapper { double res_; // Resolution used for both maps // Possible voxel values taken from VoxelMap.msg - int8_t val_free_ = kr_planning_msgs::VoxelMap::val_free; - int8_t val_occ_ = kr_planning_msgs::VoxelMap::val_occ; - int8_t val_unknown_ = kr_planning_msgs::VoxelMap::val_unknown; - int8_t val_even_ = kr_planning_msgs::VoxelMap::val_even; - int8_t val_default_ = kr_planning_msgs::VoxelMap::val_default; + int8_t val_free_ = kr_planning_msgs::msg::VoxelMap::VAL_FREE; + int8_t val_occ_ = kr_planning_msgs::msg::VoxelMap::VAL_OCC; + int8_t val_unknown_ = kr_planning_msgs::msg::VoxelMap::VAL_UNKNOWN; + int8_t val_even_ = kr_planning_msgs::msg::VoxelMap::VAL_EVEN; + int8_t val_default_ = kr_planning_msgs::msg::VoxelMap::VAL_DEFAULT; // Be careful of overflow (should always be within -128 and 128 range) // Add val_add to the voxel whenever a point lies in it. Should always be less // than 27 to avoid overflow (should always be within -128 and 128 range) - int8_t val_add_ = kr_planning_msgs::VoxelMap::val_add; + int8_t val_add_ = kr_planning_msgs::msg::VoxelMap::VAL_ADD; // Value decay (voxels will disappear if unobserved for // ((val_occ - val_even) / val_decay times) diff --git a/autonomy_core/map_plan/mapper/package.xml b/autonomy_core/map_plan/mapper/package.xml index 490efc0e..8bfcfdc6 100644 --- a/autonomy_core/map_plan/mapper/package.xml +++ b/autonomy_core/map_plan/mapper/package.xml @@ -3,23 +3,31 @@ mapper 0.0.1 The mapper package + sikang Xu Liu Guilherme Nardari Chao Qu + Jiuzhou Lei Penn Software License - catkin + + ament_cmake - roscpp + + rclcpp pcl_ros image_geometry kr_planning_msgs kr_planning_rviz_plugins sensor_msgs depth_image_proc - tf_conversions - eigen_conversions + tf2_eigen + + motion_primitive_library + + ament_cmake + diff --git a/autonomy_core/map_plan/mapper/src/local_global_mapper.cpp b/autonomy_core/map_plan/mapper/src/local_global_mapper.cpp index 47708817..96d62b76 100644 --- a/autonomy_core/map_plan/mapper/src/local_global_mapper.cpp +++ b/autonomy_core/map_plan/mapper/src/local_global_mapper.cpp @@ -1,20 +1,33 @@ #include "mapper/local_global_mapper.h" +#include -LocalGlobalMapperNode::LocalGlobalMapperNode(const ros::NodeHandle& nh) - : nh_(nh) { +// LocalGlobalMapperNode::LocalGlobalMapperNode(const ros::NodeHandle& nh) +// : nh_(nh) { +LocalGlobalMapperNode::LocalGlobalMapperNode(const rclcpp::NodeOptions & options) + : Node("local_global_mapper", options){ initParams(); - cloud_sub = nh_.subscribe( - cloud_name_, 1, &LocalGlobalMapperNode::cloudCallback, this); - - global_map_pub = - nh_.advertise("global_voxel_map", 1, true); - storage_map_pub = - nh_.advertise("storage_voxel_map", 1, true); - local_map_pub = - nh_.advertise("local_voxel_map", 1, true); - - time_pub = nh_.advertise("/timing/mapper", 1); + // cloud_sub = nh_.subscribe( + // cloud_name_, 1, &LocalGlobalMapperNode::cloudCallback, this); + cloud_sub = this->create_subscription( + cloud_name_, 1,[this](sensor_msgs::msg::PointCloud2::SharedPtr msg) { + this->cloudCallback(msg);}); + + // global_map_pub = + // nh_.advertise("global_voxel_map", 1, true); + // storage_map_pub = + // nh_.advertise("storage_voxel_map", 1, true); + // local_map_pub = + // nh_.advertise("local_voxel_map", 1, true); + // time_pub = nh_.advertise("/timing/mapper", 1); + global_map_pub = this->create_publisher( + "global_voxel_map", 1); + storage_map_pub = this->create_publisher( + "storage_voxel_map", 1); + local_map_pub = this->create_publisher( + "local_voxel_map", 1); + time_pub = this->create_publisher( + "/timing/mapper", 1); // storage map should have same resolution and z_dim as local map storage_map_info_.resolution = local_map_info_.resolution; @@ -57,30 +70,76 @@ LocalGlobalMapperNode::LocalGlobalMapperNode(const ros::NodeHandle& nh) } void LocalGlobalMapperNode::initParams() { - nh_.param("map_frame", map_frame_, std::string("map")); - nh_.param("odom_frame", odom_frame_, std::string("odom")); - nh_.param("lidar_frame", lidar_frame_, std::string("lidar")); - nh_.param("real_robot", real_robot_, false); - nh_.param("cloud_msg", cloud_name_, std::string("cloud")); - nh_.param("occ_map_height", occ_map_height_, 2.0); - nh_.param("robot_r", robot_r_, 0.2); - nh_.param("robot_h", robot_h_, 0.0); + // nh_.param("map_frame", map_frame_, std::string("map")); + // nh_.param("odom_frame", odom_frame_, std::string("odom")); + // nh_.param("lidar_frame", lidar_frame_, std::string("lidar")); + // nh_.param("real_robot", real_robot_, false); + // nh_.param("cloud_msg", cloud_name_, std::string("cloud")); + // nh_.param("occ_map_height", occ_map_height_, 2.0); + // nh_.param("robot_r", robot_r_, 0.2); + // nh_.param("robot_h", robot_h_, 0.0); + this->declare_parameter("map_frame", "map"); + map_frame_ = this->get_parameter("map_frame").as_string(); + this->declare_parameter("odom_frame", "odom"); + odom_frame_ = this->get_parameter("odom_frame").as_string(); + this->declare_parameter("lidar_frame", "lidar"); + lidar_frame_ = this->get_parameter("lidar_frame").as_string(); + this->declare_parameter("real_robot", false); + real_robot_ = this->get_parameter("real_robot").as_bool(); + this->declare_parameter("cloud_msg", "cloud"); + cloud_name_ = this->get_parameter("cloud_msg").as_string(); + this->declare_parameter("occ_map_height", 2.0); + occ_map_height_ = this->get_parameter("occ_map_height").as_double(); + this->declare_parameter("robot_r", 0.2); + robot_r_ = this->get_parameter("robot_r").as_double(); + this->declare_parameter("robot_h", 0.0); + robot_h_ = this->get_parameter("robot_h").as_double(); double global_map_cx, global_map_cy, global_map_cz; - nh_.param("global/resolution", global_map_info_.resolution, 2.0f); - nh_.param("global/center_x", global_map_cx, 0.0); - nh_.param("global/center_y", global_map_cy, 0.0); - nh_.param("global/center_z", global_map_cz, 0.0); - nh_.param("global/range_x", global_map_dim_d_x_, 500.0); - nh_.param("global/range_y", global_map_dim_d_y_, 500.0); - nh_.param("global/range_z", global_map_dim_d_z_, 2.0); - nh_.param("global/decay_times_to_empty", global_decay_times_to_empty_, 0); + // nh_.param("global/resolution", global_map_info_.resolution, 2.0f); + // nh_.param("global/center_x", global_map_cx, 0.0); + // nh_.param("global/center_y", global_map_cy, 0.0); + // nh_.param("global/center_z", global_map_cz, 0.0); + // nh_.param("global/range_x", global_map_dim_d_x_, 500.0); + // nh_.param("global/range_y", global_map_dim_d_y_, 500.0); + // nh_.param("global/range_z", global_map_dim_d_z_, 2.0); + // nh_.param("global/decay_times_to_empty", global_decay_times_to_empty_, 0); + this->declare_parameter("global/resolution", 2.0); + global_map_info_.resolution = this->get_parameter("global/resolution").as_double(); + this->declare_parameter("global/center_x", 0.0); + global_map_cx = this->get_parameter("global/center_x").as_double(); + this->declare_parameter("global/center_y", 0.0); + global_map_cy = this->get_parameter("global/center_y").as_double(); + this->declare_parameter("global/center_z", 0.0); + global_map_cz = this->get_parameter("global/center_z").as_double(); + this->declare_parameter("global/range_x", 500.0); + global_map_dim_d_x_ = + this->get_parameter("global/range_x").as_double(); + this->declare_parameter("global/range_y", 500.0); + global_map_dim_d_y_ = + this->get_parameter("global/range_y").as_double(); + this->declare_parameter("global/range_z", 2.0); + global_map_dim_d_z_ = + this->get_parameter("global/range_z").as_double(); + this->declare_parameter("global/decay_times_to_empty", 0); + global_decay_times_to_empty_ = + this->get_parameter("global/decay_times_to_empty").as_int(); // only update voxel once every update_interval_ point clouds - nh_.param("global/num_point_cloud_skip", update_interval_, 5); // int - nh_.param("global/max_raycast_range", global_max_raycast_, 100.0); - nh_.param("global/dilate_xy", global_use_robot_dim_xy_, true); - nh_.param("global/dilate_z", global_use_robot_dim_z_, false); + // nh_.param("global/num_point_cloud_skip", update_interval_, 5); // int + // nh_.param("global/max_raycast_range", global_max_raycast_, 100.0); + // nh_.param("global/dilate_xy", global_use_robot_dim_xy_, true); + // nh_.param("global/dilate_z", global_use_robot_dim_z_, false); + this->declare_parameter("global/num_point_cloud_skip", 5); + update_interval_ = + this->get_parameter("global/num_point_cloud_skip").as_int(); + this->declare_parameter("global/max_raycast_range", 100.0); + global_max_raycast_ = + this->get_parameter("global/max_raycast_range").as_double(); + this->declare_parameter("global/dilate_xy", true); + global_use_robot_dim_xy_ = + this->get_parameter("global/dilate_xy").as_bool(); + this->declare_parameter("global/dilate_z", false); // map origin is the left lower corner of the voxel map, therefore, adding // an offset make the map centered around the given position @@ -94,17 +153,37 @@ void LocalGlobalMapperNode::initParams() { global_map_info_.dim.z = static_cast( ceil((global_map_dim_d_z_) / global_map_info_.resolution)); - nh_.param("local/resolution", local_map_info_.resolution, 0.25f); - nh_.param("local/range_x", local_map_dim_d_x_, 20.0); - nh_.param("local/range_y", local_map_dim_d_y_, 20.0); - nh_.param("local/range_z", local_map_dim_d_z_, 10.0); - nh_.param("local/max_raycast_range", local_max_raycast_, 20.0); - nh_.param("local/decay_times_to_empty", local_decay_times_to_empty_, 0); + // nh_.param("local/resolution", local_map_info_.resolution, 0.25f); + // nh_.param("local/range_x", local_map_dim_d_x_, 20.0); + // nh_.param("local/range_y", local_map_dim_d_y_, 20.0); + // nh_.param("local/range_z", local_map_dim_d_z_, 10.0); + // nh_.param("local/max_raycast_range", local_max_raycast_, 20.0); + // nh_.param("local/decay_times_to_empty", local_decay_times_to_empty_, 0); + this->declare_parameter("local/resolution", 0.25); + local_map_info_.resolution = + this->get_parameter("local/resolution").as_double(); + this->declare_parameter("local/range_x", 20.0); + local_map_dim_d_x_ = + this->get_parameter("local/range_x").as_double(); + this->declare_parameter("local/range_y", 20.0); + local_map_dim_d_y_ = + this->get_parameter("local/range_y").as_double(); + this->declare_parameter("local/range_z", 10.0); + local_map_dim_d_z_ = + this->get_parameter("local/range_z").as_double(); + this->declare_parameter("local/max_raycast_range", 20.0); + local_max_raycast_ = + this->get_parameter("local/max_raycast_range").as_double(); + this->declare_parameter("local/decay_times_to_empty", 0); + local_decay_times_to_empty_ = + this->get_parameter("local/decay_times_to_empty").as_int(); } void LocalGlobalMapperNode::globalMapInit() { // TODO(xu): combine two parts into one. - ROS_WARN("[Mapper]: get 3D map info!"); + // ROS_WARN("[Mapper]: get 3D map info!"); + RCLCPP_WARN(this->get_logger(), "[Mapper]: got 3D map info!"); + // part1: global const Eigen::Vector3d global_origin(global_map_info_.origin.x, global_map_info_.origin.y, @@ -120,6 +199,7 @@ void LocalGlobalMapperNode::globalMapInit() { new mapper::VoxelMapper(global_origin, global_dim_d, global_res, + this->get_logger(), global_val_default, global_decay_times_to_empty_)); @@ -160,6 +240,7 @@ void LocalGlobalMapperNode::storageMapInit() { new mapper::VoxelMapper(storage_origin, storage_dim_d, res, + this->get_logger(), storage_val_default, local_decay_times_to_empty_)); } @@ -191,8 +272,10 @@ void LocalGlobalMapperNode::cropLocalMap( local_origin_map(2) = storage_map_info_.origin.z; // core function: crop local map from the storage map - kr_planning_msgs::VoxelMap local_voxel_map = - storage_voxel_mapper_->getInflatedLocalMap(local_origin_map, local_dim_d); + // kr_planning_msgs::msg::VoxelMap local_voxel_map = + // storage_voxel_mapper_->getInflatedLocalMap(local_origin_map, local_dim_d); + kr_planning_msgs::msg::VoxelMap local_voxel_map = + storage_voxel_mapper_->getInflatedLocalMap(local_origin_map, local_dim_d); // Transform local map by moving its origin. This is because we want the // cropped map to be centered around lidar's position wrt odom frame @@ -206,15 +289,16 @@ void LocalGlobalMapperNode::cropLocalMap( // (since we already applied transform between odom and map frame already to // compensate for the drift) local_voxel_map.header.frame_id = map_frame_; - local_map_pub.publish(local_voxel_map); + // local_map_pub.publish(local_voxel_map); + local_map_pub->publish(local_voxel_map); } void LocalGlobalMapperNode::getLidarPoses( - const std_msgs::Header& cloud_header, - geometry_msgs::Pose* pose_map_lidar_ptr, - geometry_msgs::Pose* pose_odom_lidar_ptr) { + const std_msgs::msg::Header& cloud_header, + geometry_msgs::msg::Pose* pose_map_lidar_ptr, + geometry_msgs::msg::Pose* pose_odom_lidar_ptr) { // get the transform from fixed frame to lidar frame - static mapper::TFListener tf_listener; + static mapper::TFListener tf_listener(this->shared_from_this()); if (real_robot_) { // for real robot, the point cloud frame_id may not exist in the tf tree, // manually defining it here. @@ -224,7 +308,7 @@ void LocalGlobalMapperNode::getLidarPoses( auto tf_odom_lidar = tf_listener.LookupTransform( odom_frame_, lidar_frame_, cloud_header.stamp); if ((!tf_map_lidar) || (!tf_odom_lidar)) { - ROS_WARN( + RCLCPP_WARN(this->get_logger(), "[Mapper real-robot:] Failed to get transform (either from %s to %s; " "or from %s to %s)", lidar_frame_.c_str(), @@ -242,14 +326,14 @@ void LocalGlobalMapperNode::getLidarPoses( auto tf_odom_lidar = tf_listener.LookupTransform( odom_frame_, cloud_header.frame_id, cloud_header.stamp); if (!tf_map_lidar) { - ROS_WARN( + RCLCPP_WARN(this->get_logger(), "[Mapper simulation:] Failed to get transform map to lidar (from %s " "to %s)", cloud_header.frame_id.c_str(), map_frame_.c_str()); return; } else if (!tf_odom_lidar) { - ROS_WARN( + RCLCPP_WARN(this->get_logger(), "[Mapper simulation:] Failed to get transform odom to lidar (from %s " "to %s)", cloud_header.frame_id.c_str(), @@ -262,14 +346,15 @@ void LocalGlobalMapperNode::getLidarPoses( } } -void LocalGlobalMapperNode::processCloud(const sensor_msgs::PointCloud& cloud) { +// void LocalGlobalMapperNode::processCloud(const sensor_msgs::PointCloud& cloud) { + void LocalGlobalMapperNode::processCloud(const sensor_msgs::msg::PointCloud& cloud) { if ((storage_voxel_mapper_ == nullptr) || (global_voxel_mapper_ == nullptr)) { - ROS_WARN("voxel mapper not initialized!"); + RCLCPP_WARN(this->get_logger(),"voxel mapper not initialized!"); return; } - geometry_msgs::Pose pose_map_lidar; - geometry_msgs::Pose pose_odom_lidar; + geometry_msgs::msg::Pose pose_map_lidar; + geometry_msgs::msg::Pose pose_odom_lidar; getLidarPoses(cloud.header, &pose_map_lidar, &pose_odom_lidar); const Eigen::Affine3d T_map_lidar = kr::toTF(pose_map_lidar); @@ -287,7 +372,8 @@ void LocalGlobalMapperNode::processCloud(const sensor_msgs::PointCloud& cloud) { T_odom_lidar.translation().y(), T_odom_lidar.translation().z()); - ros::Time t0; + // ros::Time t0; + rclcpp::Time t0; cpu_times tc; double min_range = 0.75; // points within this distance will be discarded @@ -299,22 +385,25 @@ void LocalGlobalMapperNode::processCloud(const sensor_msgs::PointCloud& cloud) { // local raytracing using lidar position in the map frame (not odom frame) storage_voxel_mapper_->addCloud( pts, T_map_lidar, local_infla_array_, false, local_max_raycast_); - ROS_DEBUG("[storage map addCloud]: %f", + RCLCPP_DEBUG(this->get_logger(),"[storage map addCloud]: %f", static_cast(timer.elapsed().wall) / 1e6); // get and publish storage map (this is very slow) if (pub_storage_map_) { - kr_planning_msgs::VoxelMap storage_map = + // kr_planning_msgs::msg::VoxelMap storage_map = + // storage_voxel_mapper_->getInflatedMap(); + kr_planning_msgs::msg::VoxelMap storage_map = storage_voxel_mapper_->getInflatedMap(); storage_map.header.frame_id = map_frame_; - storage_map_pub.publish(storage_map); + // storage_map_pub.publish(storage_map); + storage_map_pub->publish(storage_map); } timer.start(); // crop local voxel map // the cropping step is using the lidar position in map frame cropLocalMap(lidar_position_map, lidar_position_odom); - ROS_DEBUG("[local map crop]: %f", + RCLCPP_WARN(this->get_logger(),"[local map crop]: %f", static_cast(timer.elapsed().wall) / 1e6); // only global voxel map once every update_interval_ point clouds @@ -323,44 +412,64 @@ void LocalGlobalMapperNode::processCloud(const sensor_msgs::PointCloud& cloud) { timer.start(); global_voxel_mapper_->addCloud( pts, T_map_lidar, global_infla_array_, false, global_max_raycast_); - ROS_DEBUG("[global map addCloud]: %f", + RCLCPP_WARN(this->get_logger(),"[global map addCloud]: %f", static_cast(timer.elapsed().wall) / 1e6); timer.start(); // for global map, free voxels surrounding the robot to make sure the start // of the global planner is not occupied // global_voxel_mapper_->freeVoxels(lidar_position_map, clear_ns_); - ROS_DEBUG("[global map freeVoxels]: %f", + RCLCPP_WARN(this->get_logger(),"[global map freeVoxels]: %f", static_cast(timer.elapsed().wall) / 1e6); timer.start(); - ROS_DEBUG("[global map getInflatedMap]: %f", + RCLCPP_WARN(this->get_logger(),"[global map getInflatedMap]: %f", static_cast(timer.elapsed().wall) / 1e6); counter_ = 0; - kr_planning_msgs::VoxelMap global_map = + kr_planning_msgs::msg::VoxelMap global_map = global_voxel_mapper_->getInflatedMap(); global_map.header.frame_id = map_frame_; - global_map_pub.publish(global_map); + // global_map_pub.publish(global_map); + global_map_pub->publish(global_map); } - ROS_DEBUG_THROTTLE( + RCLCPP_DEBUG_THROTTLE(this->get_logger(), *this, 1, "[Mapper]: Got cloud, number of points: [%zu]", cloud.points.size()); } +// void LocalGlobalMapperNode::cloudCallback( +// const sensor_msgs::PointCloud2::ConstPtr& msg) { void LocalGlobalMapperNode::cloudCallback( - const sensor_msgs::PointCloud2::ConstPtr& msg) { - ROS_WARN_ONCE("[Mapper]: got the point cloud!"); - sensor_msgs::PointCloud cloud; + const sensor_msgs::msg::PointCloud2::SharedPtr msg) { + RCLCPP_WARN_ONCE(this->get_logger(),"[Mapper]: got the point cloud!"); + sensor_msgs::msg::PointCloud cloud; sensor_msgs::convertPointCloud2ToPointCloud(*msg, cloud); processCloud(cloud); } -int main(int argc, char** argv) { - ros::init(argc, argv, "local_global_mapper"); - ros::NodeHandle nh("~"); +// int main(int argc, char** argv) { +// ros::init(argc, argv, "local_global_mapper"); +// ros::NodeHandle nh("~"); + +// LocalGlobalMapperNode lgMapper(nh); +// ros::spin(); +// return 0; +// } + +int main(int argc, char** argv) +{ + // 1) Initialize the ROS 2 client library + rclcpp::init(argc, argv); + + // 2) Create your node (we forward default NodeOptions here, + // but you could customize namespace, parameters, etc.) + auto lgMapper = std::make_shared(rclcpp::NodeOptions()); + + // 3) Spin it until shutdown + rclcpp::spin(lgMapper); - LocalGlobalMapperNode lgMapper(nh); - ros::spin(); + // 4) Shutdown ROS 2 cleanly + rclcpp::shutdown(); return 0; } diff --git a/autonomy_core/map_plan/mapper/src/tf_listener.cpp b/autonomy_core/map_plan/mapper/src/tf_listener.cpp index 1e3e2bd5..dc7bb47f 100644 --- a/autonomy_core/map_plan/mapper/src/tf_listener.cpp +++ b/autonomy_core/map_plan/mapper/src/tf_listener.cpp @@ -2,20 +2,23 @@ namespace mapper { -std::optional TFListener::LookupTransform( - const std::string &target, const std::string &source, - const ros::Time &time) { - geometry_msgs::TransformStamped transformStamped; +// std::optional TFListener::LookupTransform( +// const std::string &target, const std::string &source, +// const ros::Time &time) { +std::optional TFListener::LookupTransform( + const std::string& target, + const std::string& source, + const rclcpp::Time& time){ + geometry_msgs::msg::TransformStamped transformStamped; try { - transformStamped = - buffer_.lookupTransform(target, source, time, ros::Duration(0.4)); + transformStamped = buffer_->lookupTransform(target, source, time, rclcpp::Duration::from_seconds(0.4)); } catch (tf2::TransformException &ex) { - ROS_WARN_THROTTLE(1, "Fail to find transform from [%s] to [%s]", + RCLCPP_WARN(node_->get_logger(), "Fail to find transform from [%s] to [%s]", source.c_str(), target.c_str()); return {}; } - geometry_msgs::Pose pose; + geometry_msgs::msg::Pose pose; pose.position.x = transformStamped.transform.translation.x; pose.position.y = transformStamped.transform.translation.y; diff --git a/autonomy_core/map_plan/mapper/src/voxel_mapper.cpp b/autonomy_core/map_plan/mapper/src/voxel_mapper.cpp index b874c99c..4a086c4f 100644 --- a/autonomy_core/map_plan/mapper/src/voxel_mapper.cpp +++ b/autonomy_core/map_plan/mapper/src/voxel_mapper.cpp @@ -1,12 +1,12 @@ #include "mapper/voxel_mapper.h" -#include namespace mapper { VoxelMapper::VoxelMapper(const Eigen::Vector3d& origin, const Eigen::Vector3d& dim, double res, + const rclcpp::Logger& logger, int8_t default_val, int decay_times_to_empty) { origin_d_ = Eigen::Vector3d::Zero(); @@ -25,7 +25,7 @@ VoxelMapper::VoxelMapper(const Eigen::Vector3d& origin, int val_temp = val_occ_ + val_add_; if (val_temp > 128) { - ROS_ERROR_STREAM("val_occ + val_add is larger than 128, the value is: " + RCLCPP_ERROR_STREAM(logger, "val_occ + val_add is larger than 128, the value is: " << val_temp << ", this will cause overflow!!! " "Reducing it to < 128 now..."); @@ -50,8 +50,8 @@ void VoxelMapper::setMapFree() { inflated_map_.setMap(origin_d_, dim_, base_map, res_); } -kr_planning_msgs::VoxelMap VoxelMapper::getMap() { - kr_planning_msgs::VoxelMap voxel_map; +kr_planning_msgs::msg::VoxelMap VoxelMapper::getMap() { + kr_planning_msgs::msg::VoxelMap voxel_map; voxel_map.origin.x = origin_d_(0); voxel_map.origin.y = origin_d_(1); voxel_map.origin.z = origin_d_(2); @@ -76,8 +76,8 @@ kr_planning_msgs::VoxelMap VoxelMapper::getMap() { return voxel_map; } -kr_planning_msgs::VoxelMap VoxelMapper::getInflatedMap() { - kr_planning_msgs::VoxelMap voxel_map; +kr_planning_msgs::msg::VoxelMap VoxelMapper::getInflatedMap() { + kr_planning_msgs::msg::VoxelMap voxel_map; voxel_map.origin.x = origin_d_(0); voxel_map.origin.y = origin_d_(1); voxel_map.origin.z = origin_d_(2); @@ -102,9 +102,9 @@ kr_planning_msgs::VoxelMap VoxelMapper::getInflatedMap() { return voxel_map; } -kr_planning_msgs::VoxelMap VoxelMapper::getInflatedLocalMap( +kr_planning_msgs::msg::VoxelMap VoxelMapper::getInflatedLocalMap( const Eigen::Vector3d& ori_d, const Eigen::Vector3d& dim_d) { - kr_planning_msgs::VoxelMap voxel_map; + kr_planning_msgs::msg::VoxelMap voxel_map; voxel_map.resolution = res_; voxel_map.origin.x = ori_d(0); @@ -157,9 +157,9 @@ kr_planning_msgs::VoxelMap VoxelMapper::getInflatedLocalMap( // TODO(xu): This function is the same as sliceMap function in // data_conversions.cpp, should merge them. -kr_planning_msgs::VoxelMap VoxelMapper::getInflatedOccMap(double h, +kr_planning_msgs::msg::VoxelMap VoxelMapper::getInflatedOccMap(double h, double hh) { - kr_planning_msgs::VoxelMap voxel_map; + kr_planning_msgs::msg::VoxelMap voxel_map; voxel_map.origin.x = origin_d_(0); voxel_map.origin.y = origin_d_(1); voxel_map.origin.z = 0; diff --git a/autonomy_core/map_plan/mpl/CMakeLists.txt b/autonomy_core/map_plan/mpl/CMakeLists.txt index c1191454..181a6c21 100644 --- a/autonomy_core/map_plan/mpl/CMakeLists.txt +++ b/autonomy_core/map_plan/mpl/CMakeLists.txt @@ -1,45 +1,45 @@ -cmake_minimum_required(VERSION 3.10) -project(motion_primitive_library) +# cmake_minimum_required(VERSION 3.10) +# project(motion_primitive_library) -set(CMAKE_CXX_STANDARD 17) +# set(CMAKE_CXX_STANDARD 17) -find_package(catkin REQUIRED - COMPONENTS kr_planning_msgs) -find_package(Eigen3 REQUIRED) -find_package(Boost REQUIRED) +# find_package(catkin REQUIRED +# COMPONENTS kr_planning_msgs) +# find_package(Eigen3 REQUIRED) +# find_package(Boost REQUIRED) -catkin_package( - INCLUDE_DIRS include - LIBRARIES ${PROJECT_NAME} - CATKIN_DEPENDS kr_planning_msgs - DEPENDS EIGEN3) +# catkin_package( +# INCLUDE_DIRS include +# LIBRARIES ${PROJECT_NAME} +# CATKIN_DEPENDS kr_planning_msgs +# DEPENDS EIGEN3) -add_library( - ${PROJECT_NAME} - src/math.cpp - src/lambda.cpp - src/primitive.cpp - src/trajectory.cpp - src/waypoint.cpp - src/map_util.cpp - src/env_base.cpp - src/env_map.cpp - src/map_planner.cpp - src/planner_base.cpp - src/state_space.cpp - src/graph_search.cpp) -target_include_directories(${PROJECT_NAME} PUBLIC include ${catkin_INCLUDE_DIRS}) -target_link_libraries(${PROJECT_NAME} PUBLIC Eigen3::Eigen Boost::boost) +# add_library( +# ${PROJECT_NAME} +# src/math.cpp +# src/lambda.cpp +# src/primitive.cpp +# src/trajectory.cpp +# src/waypoint.cpp +# src/map_util.cpp +# src/env_base.cpp +# src/env_map.cpp +# src/map_planner.cpp +# src/planner_base.cpp +# src/state_space.cpp +# src/graph_search.cpp) +# target_include_directories(${PROJECT_NAME} PUBLIC include ${catkin_INCLUDE_DIRS}) +# target_link_libraries(${PROJECT_NAME} PUBLIC Eigen3::Eigen Boost::boost) -#---------------------------------------------------------------------------------- -# Add testing with GoogleTest +# #---------------------------------------------------------------------------------- +# # Add testing with GoogleTest -# To run test: -# catkin test motion_primitive_library -enable_testing() +# # To run test: +# # catkin test motion_primitive_library +# enable_testing() -catkin_add_gtest(test_map_util test/test_map_util.cpp) -target_link_libraries(test_map_util ${catkin_LIBRARIES} ${PROJECT_NAME}) +# catkin_add_gtest(test_map_util test/test_map_util.cpp) +# target_link_libraries(test_map_util ${catkin_LIBRARIES} ${PROJECT_NAME}) #---------------------------------------------------------------------------------- # Add microbenchmarking with Google Benchmark @@ -63,4 +63,71 @@ target_link_libraries(test_map_util ${catkin_LIBRARIES} ${PROJECT_NAME}) # FetchContent_MakeAvailable(benchmark) # add_executable(map_util_benchmarking test/benchmark_map_util.cpp) -# target_link_libraries(map_util_benchmarking benchmark::benchmark ${PROJECT_NAME}) \ No newline at end of file +# target_link_libraries(map_util_benchmarking benchmark::benchmark ${PROJECT_NAME}) + + +# ROS2 VERSION + +cmake_minimum_required(VERSION 3.10) +project(motion_primitive_library) + +set(CMAKE_CXX_STANDARD 17) + +find_package(ament_cmake REQUIRED) +find_package(kr_planning_msgs REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(Boost REQUIRED) + +add_library( + ${PROJECT_NAME} + src/math.cpp + src/lambda.cpp + src/primitive.cpp + src/trajectory.cpp + src/waypoint.cpp + src/map_util.cpp + src/env_base.cpp + src/env_map.cpp + src/map_planner.cpp + src/planner_base.cpp + src/state_space.cpp + src/graph_search.cpp) + +ament_target_dependencies(${PROJECT_NAME} kr_planning_msgs + Eigen3 Boost) +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $ + ${EIGEN3_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} +) + +# ---------------------------------------- +# Install targets +# ---------------------------------------- +install(TARGETS ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin) + +install(DIRECTORY include/ + DESTINATION include) + +# ---------------------------------------- +# Export for downstream use +# ---------------------------------------- +ament_export_targets(export_${PROJECT_NAME}) +ament_export_dependencies(kr_planning_msgs Eigen3 Boost) + +#---------------------------------------------------------------------------------- +# Add testing with GoogleTest + +# To run test: +# catkin test motion_primitive_library +# enable_testing() + +# catkin_add_gtest(test_map_util test/test_map_util.cpp) +# target_link_libraries(test_map_util ${catkin_LIBRARIES} ${PROJECT_NAME}) + +ament_package() \ No newline at end of file diff --git a/autonomy_core/map_plan/mpl/include/mpl_collision/map_util.h b/autonomy_core/map_plan/mpl/include/mpl_collision/map_util.h index 683eda22..e0f6cfc9 100644 --- a/autonomy_core/map_plan/mpl/include/mpl_collision/map_util.h +++ b/autonomy_core/map_plan/mpl/include/mpl_collision/map_util.h @@ -1,7 +1,7 @@ #pragma once #include "mpl_basis/data_type.h" -#include +#include #include namespace MPL { @@ -77,7 +77,7 @@ class MapUtil { /// Get unknown voxels for 3D vec_Vecf getUnknownCloud(); - kr_planning_msgs::VoxelMap voxel_map; + kr_planning_msgs::msg::VoxelMap voxel_map; protected: /// Map resolution @@ -91,13 +91,13 @@ class MapUtil { /// 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; diff --git a/autonomy_core/map_plan/mpl/package.xml b/autonomy_core/map_plan/mpl/package.xml index c9a9179f..cbeebc12 100644 --- a/autonomy_core/map_plan/mpl/package.xml +++ b/autonomy_core/map_plan/mpl/package.xml @@ -4,13 +4,17 @@ search-based motion primitive planning library Sikang Liu Sikang Liu + Jiuzhou Lei Apache-2.0 - catkin + ament_cmake kr_planning_msgs + Eigen3 + Boost + ament_cmake diff --git a/autonomy_core/map_plan/traj_opt_ros/CMakeLists.txt b/autonomy_core/map_plan/traj_opt_ros/CMakeLists.txt index 1b569912..729cbb32 100644 --- a/autonomy_core/map_plan/traj_opt_ros/CMakeLists.txt +++ b/autonomy_core/map_plan/traj_opt_ros/CMakeLists.txt @@ -3,17 +3,42 @@ project(traj_opt_ros) set(CMAKE_CXX_STANDARD 17) -find_package(catkin REQUIRED COMPONENTS roscpp kr_planning_msgs) -find_package(Eigen3 REQUIRED NO_MODULE) - -catkin_package( - INCLUDE_DIRS include - LIBRARIES ${PROJECT_NAME} - CATKIN_DEPENDS kr_planning_msgs - DEPENDS EIGEN3) - -add_library(${PROJECT_NAME} src/ros_bridge.cpp src/msg_traj.cpp - src/polynomial_basis.cpp src/trajectory.cpp) -target_include_directories(${PROJECT_NAME} PUBLIC include - ${catkin_INCLUDE_DIRS}) -target_link_libraries(${PROJECT_NAME} PUBLIC ${catkin_LIBRARIES} Eigen3::Eigen) +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(kr_planning_msgs REQUIRED) +find_package(Eigen3 REQUIRED) + +# Declare library +add_library(${PROJECT_NAME} + src/ros_bridge.cpp + src/msg_traj.cpp + src/polynomial_basis.cpp + src/trajectory.cpp +) + +target_include_directories(${PROJECT_NAME} PUBLIC + include + $ + $ +) + +target_link_libraries(${PROJECT_NAME} + Eigen3::Eigen +) + +ament_target_dependencies(${PROJECT_NAME} + rclcpp + kr_planning_msgs +) + +# Install targets +install(TARGETS ${PROJECT_NAME} + LIBRARY DESTINATION lib/${PROJECT_NAME} +) + +install(DIRECTORY include/ + DESTINATION include/${PROJECT_NAME} +) + +ament_package() + diff --git a/autonomy_core/map_plan/traj_opt_ros/include/traj_opt_ros/ros_bridge.h b/autonomy_core/map_plan/traj_opt_ros/include/traj_opt_ros/ros_bridge.h index 44520225..dd90c3f2 100644 --- a/autonomy_core/map_plan/traj_opt_ros/include/traj_opt_ros/ros_bridge.h +++ b/autonomy_core/map_plan/traj_opt_ros/include/traj_opt_ros/ros_bridge.h @@ -1,9 +1,10 @@ // Copyright 2016 Michael Watterson #pragma once -#include -#include -#include +#include +#include +// #include +#include #include @@ -11,10 +12,10 @@ namespace traj_opt { -class TrajRosBridge { +class TrajRosBridge: public rclcpp::Node { public: // make sure to run ros::init() before calling this function or it won't work - static void publish_msg(const kr_planning_msgs::SplineTrajectory &msg, + static void publish_msg(const kr_planning_msgs::msg::SplineTrajectory &msg, std::string frame_id = "map"); static void publish_msg(const TrajData &data, std::string frame_id = "map"); @@ -24,17 +25,18 @@ class TrajRosBridge { private: TrajRosBridge(); static TrajRosBridge &instance(); - ros::NodeHandle nh_; - ros::Publisher pub_; +// ros::NodeHandle nh_; +// ros::Publisher pub_; + rclcpp::Publisher::SharedPtr pub_; }; -kr_planning_msgs::SplineTrajectory SplineTrajectoryFromTrajData( +kr_planning_msgs::msg::SplineTrajectory SplineTrajectoryFromTrajData( const TrajData &data); TrajData TrajDataFromSplineTrajectory( - const kr_planning_msgs::SplineTrajectory &msg); + const kr_planning_msgs::msg::SplineTrajectory &msg); -kr_planning_msgs::SplineTrajectory SplineTrajectoryFromTrajectory( - const kr_planning_msgs::Trajectory &msg); +kr_planning_msgs::msg::SplineTrajectory SplineTrajectoryFromTrajectory( + const kr_planning_msgs::msg::Trajectory &msg); } // namespace traj_opt diff --git a/autonomy_core/map_plan/traj_opt_ros/package.xml b/autonomy_core/map_plan/traj_opt_ros/package.xml index 57dcd8f3..9a08e26e 100644 --- a/autonomy_core/map_plan/traj_opt_ros/package.xml +++ b/autonomy_core/map_plan/traj_opt_ros/package.xml @@ -5,16 +5,18 @@ The traj_opt_ros package Mike Watterson + Jiuzhou Lei Penn Software License - catkin + ament_cmake - roscpp + rclcpp kr_planning_msgs + Eigen3 - + ament_cmake diff --git a/autonomy_core/map_plan/traj_opt_ros/src/ros_bridge.cpp b/autonomy_core/map_plan/traj_opt_ros/src/ros_bridge.cpp index eb7fd209..7463908f 100644 --- a/autonomy_core/map_plan/traj_opt_ros/src/ros_bridge.cpp +++ b/autonomy_core/map_plan/traj_opt_ros/src/ros_bridge.cpp @@ -3,38 +3,40 @@ namespace traj_opt { -TrajRosBridge::TrajRosBridge() : nh_("~") { - pub_ = - nh_.advertise("trajectory", 1, true); +TrajRosBridge::TrajRosBridge(): rclcpp::Node("traj_ros_bridge") { + // pub_ = + // nh_.advertise("trajectory", 1, true); + pub_ = this->create_publisher( + "trajectory", 1); } TrajRosBridge &TrajRosBridge::instance() { static TrajRosBridge inst; return inst; } -void TrajRosBridge::publish_msg(const kr_planning_msgs::SplineTrajectory &msg, +void TrajRosBridge::publish_msg(const kr_planning_msgs::msg::SplineTrajectory &msg, std::string frame_id) { - kr_planning_msgs::SplineTrajectory msgc = msg; + kr_planning_msgs::msg::SplineTrajectory msgc = msg; msgc.header.frame_id = frame_id; - instance().pub_.publish(msgc); + instance().pub_->publish(msgc); } void TrajRosBridge::publish_msg(const TrajData &data, std::string frame_id) { publish_msg(SplineTrajectoryFromTrajData(data), frame_id); } // these convert functions can be written more cleanly with templates -kr_planning_msgs::SplineTrajectory SplineTrajectoryFromTrajData( +kr_planning_msgs::msg::SplineTrajectory SplineTrajectoryFromTrajData( const TrajData &data) { - kr_planning_msgs::SplineTrajectory traj; - traj.header.stamp = ros::Time::now(); + kr_planning_msgs::msg::SplineTrajectory traj; + traj.header.stamp = rclcpp::Clock().now(); traj.header.frame_id = "map"; traj.dimension_names = data.dimension_names; traj.dimensions = data.dimensions; // copy all fields for (auto spline : data.data) { - kr_planning_msgs::Spline s; + kr_planning_msgs::msg::Spline s; for (auto poly : spline.segs) { - kr_planning_msgs::Polynomial p; + kr_planning_msgs::msg::Polynomial p; p.degree = poly.degree; p.dt = poly.dt; p.basis = poly.basis; @@ -49,7 +51,7 @@ kr_planning_msgs::SplineTrajectory SplineTrajectoryFromTrajData( } TrajData TrajDataFromSplineTrajectory( - const kr_planning_msgs::SplineTrajectory &msg) { + const kr_planning_msgs::msg::SplineTrajectory &msg) { TrajData data; // copy all fields data.dimension_names = msg.dimension_names; @@ -93,9 +95,9 @@ int Factorial(int n) { return results[n]; // undefined behavior if n > 12 } -kr_planning_msgs::SplineTrajectory SplineTrajectoryFromTrajectory( - const kr_planning_msgs::Trajectory &msg) { - kr_planning_msgs::SplineTrajectory traj; +kr_planning_msgs::msg::SplineTrajectory SplineTrajectoryFromTrajectory( + const kr_planning_msgs::msg::Trajectory &msg) { + kr_planning_msgs::msg::SplineTrajectory traj; traj.header = msg.header; double T = 0.0; @@ -104,14 +106,14 @@ kr_planning_msgs::SplineTrajectory SplineTrajectoryFromTrajectory( } for (uint d = 0; d < 3; d++) { - kr_planning_msgs::Spline spline; + kr_planning_msgs::msg::Spline spline; for (uint s = 0; s < msg.primitives.size(); s++) { const std::vector *co; // get correct field if (d == 0) co = &(msg.primitives.at(s).cx); if (d == 1) co = &(msg.primitives.at(s).cy); if (d == 2) co = &(msg.primitives.at(s).cz); - kr_planning_msgs::Polynomial poly; + kr_planning_msgs::msg::Polynomial poly; for (uint c = 0; c < co->size(); c++) { uint cr = co->size() - 1 - c; poly.coeffs.push_back(co->at(cr) * std::pow(msg.primitives.at(s).t, c) / diff --git a/autonomy_sim/gazebo_sim/gazebo_utils/CMakeLists.txt b/autonomy_sim/gazebo_sim/gazebo_utils/CMakeLists.txt index 9bad131b..01d2cd0a 100644 --- a/autonomy_sim/gazebo_sim/gazebo_utils/CMakeLists.txt +++ b/autonomy_sim/gazebo_sim/gazebo_utils/CMakeLists.txt @@ -3,6 +3,9 @@ project(gazebo_utils) set(CMAKE_CXX_STANDARD 17) -find_package(catkin REQUIRED) - -catkin_package() +find_package(ament_cmake REQUIRED) +install(DIRECTORY launch/ + DESTINATION share/${PROJECT_NAME}/launch) +install(DIRECTORY config/ + DESTINATION share/${PROJECT_NAME}/config) +ament_package() diff --git a/autonomy_sim/gazebo_sim/gazebo_utils/launch/full_sim.launch.xml b/autonomy_sim/gazebo_sim/gazebo_utils/launch/full_sim.launch.xml new file mode 100644 index 00000000..4d8e718c --- /dev/null +++ b/autonomy_sim/gazebo_sim/gazebo_utils/launch/full_sim.launch.xml @@ -0,0 +1,84 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/autonomy_sim/gazebo_sim/gazebo_utils/launch/simulation.launch.xml b/autonomy_sim/gazebo_sim/gazebo_utils/launch/simulation.launch.xml new file mode 100644 index 00000000..a996cc9c --- /dev/null +++ b/autonomy_sim/gazebo_sim/gazebo_utils/launch/simulation.launch.xml @@ -0,0 +1,65 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/autonomy_sim/gazebo_sim/gazebo_utils/package.xml b/autonomy_sim/gazebo_sim/gazebo_utils/package.xml index 484ed4b9..2d4bdb36 100644 --- a/autonomy_sim/gazebo_sim/gazebo_utils/package.xml +++ b/autonomy_sim/gazebo_sim/gazebo_utils/package.xml @@ -7,10 +7,12 @@ Xu Liu Chao Qu Guilherme Nardari + Jiuzhou Lei Penn Software License - catkin + ament_cmake + ament_cmake