CV学习日志：CV开发之ROS2和Webots头文件及其自动提取

　　ROS2包含许多的开发包，这些开发包覆盖了机器人开发过程所有可能涉及到的消息、服务及动作。我们可以直接使用这些消息/服务/动作进行开发而无需再自定义，从而提高开发效率。然而，也正因为ROS2开发包较多且不同版本的开发包可能还不完全一致，所以要一览其提供的消息/服务/动作、或要自动提取相应的头文件、或要比较不同版本ROS2间的区别还比较麻烦，尤其是要经常这么做时。于是，我基于ROS2文件结构的特点写了一个脚本来自动提取每个开发包的头文件，函数createROSHPP已经包含了所有提供了消息/服务/动作的开发包及其它常用的开发包(如rclcpp/rcl_action/ament_index_cpp)，若有新增开发包或自编译包，只需要按相应的格式在函数createROSHPP中追加相应的包即可。

　　另外，由于Webots脱离ROS2的易用性、多平台的良好兼容性及开发自定义Webots与ROS2通信插件的需要，我也写了一个自动提取Webots所有CPP头文件(因为更偏好C++开发)的脚本，但函数createWebotsHPP中也实现了自动提取Webots所有C头文件的功能，只是注释掉了，若有需求也可开启。

　　以上功能都封装在类ROS2WebotsHeaders中，类主要功能如下：

　　　　TestMe：主函数(输入参数为{whichLib(只能为ros或webots) homePath(如/usr/local/webots或/opt/ros/foxy)})

　　　　createWebotsHPP：提取Webots的所有HPP头文件(去掉注释也可提取所有H头文件)

　　　　createROSHPP：提取ROS2相关包的所有HPP头文件(按相应格式追加包即可提取对应包的所有HPP头文件)

　　　　globPaths：获取所有文件或目录或文件目录

　　　　getNewPath：获取指定基名和扩展名的尚不存在的路径

　　以下自动提取的webots-2020b-rev1和ros-foxy-rev1-windows的头文件

#define stdmsg std_msgs::msg
#define stdsrv std_srvs::srv
#define geomsg geometry_msgs::msg
#define ssrmsg sensor_msgs::msg

#if 1 //Webots headers
#include <webots/GPS.hpp>
#include <webots/LED.hpp>
#include <webots/Pen.hpp>
#include <webots/Gyro.hpp>
#include <webots/Node.hpp>
#include <webots/Skin.hpp>
#include <webots/Brake.hpp>
#include <webots/Field.hpp>
#include <webots/Lidar.hpp>
#include <webots/Motor.hpp>
#include <webots/Mouse.hpp>
#include <webots/Radar.hpp>
#include <webots/Robot.hpp>
#include <webots/Camera.hpp>
#include <webots/Device.hpp>
#include <webots/Compass.hpp>
#include <webots/Display.hpp>
#include <webots/Emitter.hpp>
#include <webots/Speaker.hpp>
#include <webots/ImageRef.hpp>
#include <webots/Joystick.hpp>
#include <webots/Keyboard.hpp>
#include <webots/Receiver.hpp>
#include <webots/Connector.hpp>
#include <webots/Supervisor.hpp>
#include <webots/LightSensor.hpp>
#include <webots/RangeFinder.hpp>
#include <webots/TouchSensor.hpp>
#include <webots/vehicle/Car.hpp>
#include <webots/InertialUnit.hpp>
#include <webots/utils/Motion.hpp>
#include <webots/Accelerometer.hpp>
#include <webots/DistanceSensor.hpp>
#include <webots/PositionSensor.hpp>
#include <webots/vehicle/Driver.hpp>
#include <webots/utils/AnsiCodes.hpp>
#include <webots/DifferentialWheels.hpp>
#endif

#if 1 //ROS2 headers

//ament_index_cpp
#include <ament_index_cpp/get_resource.hpp>
#include <ament_index_cpp/has_resource.hpp>
#include <ament_index_cpp/get_resources.hpp>
#include <ament_index_cpp/get_search_paths.hpp>
#include <ament_index_cpp/get_package_prefix.hpp>
#include <ament_index_cpp/get_packages_with_prefixes.hpp>
#include <ament_index_cpp/get_package_share_directory.hpp>

//rclcpp
#include <rclcpp/qos.hpp>
#include <rclcpp/node.hpp>
#include <rclcpp/rate.hpp>
#include <rclcpp/time.hpp>
#include <rclcpp/clock.hpp>
#include <rclcpp/event.hpp>
#include <rclcpp/timer.hpp>
#include <rclcpp/client.hpp>
#include <rclcpp/logger.hpp>
#include <rclcpp/macros.hpp>
#include <rclcpp/rclcpp.hpp>
#include <rclcpp/context.hpp>
#include <rclcpp/logging.hpp>
#include <rclcpp/service.hpp>
#include <rclcpp/duration.hpp>
#include <rclcpp/executor.hpp>
#include <rclcpp/wait_set.hpp>
#include <rclcpp/waitable.hpp>
#include <rclcpp/executors.hpp>
#include <rclcpp/parameter.hpp>
#include <rclcpp/publisher.hpp>
#include <rclcpp/qos_event.hpp>
#include <rclcpp/utilities.hpp>
#include <rclcpp/exceptions.hpp>
#include <rclcpp/scope_exit.hpp>
#include <rclcpp/time_source.hpp>
#include <rclcpp/wait_result.hpp>
#include <rclcpp/create_timer.hpp>
#include <rclcpp/init_options.hpp>
#include <rclcpp/message_info.hpp>
#include <rclcpp/node_options.hpp>
#include <rclcpp/subscription.hpp>
#include <rclcpp/create_client.hpp>
#include <rclcpp/parameter_map.hpp>
#include <rclcpp/serialization.hpp>
#include <rclcpp/any_executable.hpp>
#include <rclcpp/callback_group.hpp>
#include <rclcpp/create_service.hpp>
#include <rclcpp/graph_listener.hpp>
#include <rclcpp/loaned_message.hpp>
#include <rclcpp/publisher_base.hpp>
#include <rclcpp/function_traits.hpp>
#include <rclcpp/guard_condition.hpp>
#include <rclcpp/memory_strategy.hpp>
#include <rclcpp/parameter_value.hpp>
#include <rclcpp/create_publisher.hpp>
#include <rclcpp/executor_options.hpp>
#include <rclcpp/parameter_client.hpp>
#include <rclcpp/wait_result_kind.hpp>
#include <rclcpp/memory_strategies.hpp>
#include <rclcpp/parameter_service.hpp>
#include <rclcpp/publisher_factory.hpp>
#include <rclcpp/publisher_options.hpp>
#include <rclcpp/subscription_base.hpp>
#include <rclcpp/type_support_decl.hpp>
#include <rclcpp/wait_set_template.hpp>
#include <rclcpp/future_return_code.hpp>
#include <rclcpp/serialized_message.hpp>
#include <rclcpp/visibility_control.hpp>
#include <rclcpp/create_subscription.hpp>
#include <rclcpp/subscription_traits.hpp>
#include <rclcpp/any_service_callback.hpp>
#include <rclcpp/subscription_factory.hpp>
#include <rclcpp/subscription_options.hpp>
#include <rclcpp/intra_process_setting.hpp>
#include <rclcpp/topic_statistics_state.hpp>
#include <rclcpp/message_memory_strategy.hpp>
#include <rclcpp/parameter_events_filter.hpp>
#include <rclcpp/any_subscription_callback.hpp>
#include <rclcpp/intra_process_buffer_type.hpp>
#include <rclcpp/subscription_wait_set_mask.hpp>
#include <rclcpp/expand_topic_or_service_name.hpp>

//rclcpp_action
#include <rclcpp_action/qos.hpp>
#include <rclcpp_action/types.hpp>
#include <rclcpp_action/client.hpp>
#include <rclcpp_action/server.hpp>
#include <rclcpp_action/exceptions.hpp>
#include <rclcpp_action/create_client.hpp>
#include <rclcpp_action/create_server.hpp>
#include <rclcpp_action/rclcpp_action.hpp>
#include <rclcpp_action/client_goal_handle.hpp>
#include <rclcpp_action/server_goal_handle.hpp>
#include <rclcpp_action/visibility_control.hpp>

//std_msgs
#include <std_msgs/msg/bool.hpp>
#include <std_msgs/msg/byte.hpp>
#include <std_msgs/msg/char.hpp>
#include <std_msgs/msg/int8.hpp>
#include <std_msgs/msg/empty.hpp>
#include <std_msgs/msg/int16.hpp>
#include <std_msgs/msg/int32.hpp>
#include <std_msgs/msg/int64.hpp>
#include <std_msgs/msg/header.hpp>
#include <std_msgs/msg/string.hpp>
#include <std_msgs/msg/u_int8.hpp>
#include <std_msgs/msg/float32.hpp>
#include <std_msgs/msg/float64.hpp>
#include <std_msgs/msg/u_int16.hpp>
#include <std_msgs/msg/u_int32.hpp>
#include <std_msgs/msg/u_int64.hpp>
#include <std_msgs/msg/color_rgba.hpp>
#include <std_msgs/msg/byte_multi_array.hpp>
#include <std_msgs/msg/int8_multi_array.hpp>
#include <std_msgs/msg/int16_multi_array.hpp>
#include <std_msgs/msg/int32_multi_array.hpp>
#include <std_msgs/msg/int64_multi_array.hpp>
#include <std_msgs/msg/multi_array_layout.hpp>
#include <std_msgs/msg/u_int8_multi_array.hpp>
#include <std_msgs/msg/float32_multi_array.hpp>
#include <std_msgs/msg/float64_multi_array.hpp>
#include <std_msgs/msg/u_int16_multi_array.hpp>
#include <std_msgs/msg/u_int32_multi_array.hpp>
#include <std_msgs/msg/u_int64_multi_array.hpp>
#include <std_msgs/msg/multi_array_dimension.hpp>

//std_srvs
#include <std_srvs/srv/empty.hpp>
#include <std_srvs/srv/trigger.hpp>
#include <std_srvs/srv/set_bool.hpp>

//builtin_interfaces
#include <builtin_interfaces/msg/time.hpp>
#include <builtin_interfaces/msg/duration.hpp>

//sensor_msgs
#include <sensor_msgs/fill_image.hpp>
#include <sensor_msgs/image_encodings.hpp>
#include <sensor_msgs/distortion_models.hpp>
#include <sensor_msgs/point_cloud2_iterator.hpp>
#include <sensor_msgs/point_cloud_conversion.hpp>
#include <sensor_msgs/point_field_conversion.hpp>
#include <sensor_msgs/msg/imu.hpp>
#include <sensor_msgs/msg/joy.hpp>
#include <sensor_msgs/msg/image.hpp>
#include <sensor_msgs/msg/range.hpp>
#include <sensor_msgs/msg/laser_echo.hpp>
#include <sensor_msgs/msg/laser_scan.hpp>
#include <sensor_msgs/msg/camera_info.hpp>
#include <sensor_msgs/msg/illuminance.hpp>
#include <sensor_msgs/msg/joint_state.hpp>
#include <sensor_msgs/msg/nav_sat_fix.hpp>
#include <sensor_msgs/msg/point_cloud.hpp>
#include <sensor_msgs/msg/point_field.hpp>
#include <sensor_msgs/msg/temperature.hpp>
#include <sensor_msgs/msg/joy_feedback.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <sensor_msgs/msg/battery_state.hpp>
#include <sensor_msgs/msg/fluid_pressure.hpp>
#include <sensor_msgs/msg/magnetic_field.hpp>
#include <sensor_msgs/msg/nav_sat_status.hpp>
#include <sensor_msgs/msg/time_reference.hpp>
#include <sensor_msgs/msg/channel_float32.hpp>
#include <sensor_msgs/msg/compressed_image.hpp>
#include <sensor_msgs/msg/relative_humidity.hpp>
#include <sensor_msgs/msg/joy_feedback_array.hpp>
#include <sensor_msgs/msg/region_of_interest.hpp>
#include <sensor_msgs/msg/multi_dof_joint_state.hpp>
#include <sensor_msgs/msg/multi_echo_laser_scan.hpp>
#include <sensor_msgs/srv/set_camera_info.hpp>

//geometry_msgs
#include <geometry_msgs/msg/pose.hpp>
#include <geometry_msgs/msg/accel.hpp>
#include <geometry_msgs/msg/point.hpp>
#include <geometry_msgs/msg/twist.hpp>
#include <geometry_msgs/msg/wrench.hpp>
#include <geometry_msgs/msg/inertia.hpp>
#include <geometry_msgs/msg/point32.hpp>
#include <geometry_msgs/msg/polygon.hpp>
#include <geometry_msgs/msg/pose2_d.hpp>
#include <geometry_msgs/msg/vector3.hpp>
#include <geometry_msgs/msg/transform.hpp>
#include <geometry_msgs/msg/pose_array.hpp>
#include <geometry_msgs/msg/quaternion.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <geometry_msgs/msg/accel_stamped.hpp>
#include <geometry_msgs/msg/point_stamped.hpp>
#include <geometry_msgs/msg/twist_stamped.hpp>
#include <geometry_msgs/msg/wrench_stamped.hpp>
#include <geometry_msgs/msg/inertia_stamped.hpp>
#include <geometry_msgs/msg/polygon_stamped.hpp>
#include <geometry_msgs/msg/vector3_stamped.hpp>
#include <geometry_msgs/msg/transform_stamped.hpp>
#include <geometry_msgs/msg/quaternion_stamped.hpp>
#include <geometry_msgs/msg/pose_with_covariance.hpp>
#include <geometry_msgs/msg/accel_with_covariance.hpp>
#include <geometry_msgs/msg/twist_with_covariance.hpp>
#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
#include <geometry_msgs/msg/accel_with_covariance_stamped.hpp>
#include <geometry_msgs/msg/twist_with_covariance_stamped.hpp>

//trajectory_msgs
#include <trajectory_msgs/msg/joint_trajectory.hpp>
#include <trajectory_msgs/msg/joint_trajectory_point.hpp>
#include <trajectory_msgs/msg/multi_dof_joint_trajectory.hpp>
#include <trajectory_msgs/msg/multi_dof_joint_trajectory_point.hpp>

//move_base_msgs
#include <move_base_msgs/action/move_base.hpp>

//stereo_msgs
#include <stereo_msgs/msg/disparity_image.hpp>

//shape_msgs
#include <shape_msgs/msg/mesh.hpp>
#include <shape_msgs/msg/plane.hpp>
#include <shape_msgs/msg/mesh_triangle.hpp>
#include <shape_msgs/msg/solid_primitive.hpp>

//map_msgs
#include <map_msgs/msg/projected_map.hpp>
#include <map_msgs/msg/projected_map_info.hpp>
#include <map_msgs/msg/point_cloud2_update.hpp>
#include <map_msgs/msg/occupancy_grid_update.hpp>
#include <map_msgs/srv/save_map.hpp>
#include <map_msgs/srv/get_map_roi.hpp>
#include <map_msgs/srv/get_point_map.hpp>
#include <map_msgs/srv/get_point_map_roi.hpp>
#include <map_msgs/srv/projected_maps_info.hpp>
#include <map_msgs/srv/set_map_projections.hpp>

//nav_msgs
#include <nav_msgs/msg/path.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <nav_msgs/msg/grid_cells.hpp>
#include <nav_msgs/msg/map_meta_data.hpp>
#include <nav_msgs/msg/occupancy_grid.hpp>
#include <nav_msgs/srv/get_map.hpp>
#include <nav_msgs/srv/set_map.hpp>
#include <nav_msgs/srv/get_plan.hpp>

//visualization_msgs
#include <visualization_msgs/msg/marker.hpp>
#include <visualization_msgs/msg/menu_entry.hpp>
#include <visualization_msgs/msg/image_marker.hpp>
#include <visualization_msgs/msg/marker_array.hpp>
#include <visualization_msgs/msg/interactive_marker.hpp>
#include <visualization_msgs/msg/interactive_marker_init.hpp>
#include <visualization_msgs/msg/interactive_marker_pose.hpp>
#include <visualization_msgs/msg/interactive_marker_update.hpp>
#include <visualization_msgs/msg/interactive_marker_control.hpp>
#include <visualization_msgs/msg/interactive_marker_feedback.hpp>
#include <visualization_msgs/srv/get_interactive_markers.hpp>

//diagnostic_msgs
#include <diagnostic_msgs/msg/key_value.hpp>
#include <diagnostic_msgs/msg/diagnostic_array.hpp>
#include <diagnostic_msgs/msg/diagnostic_status.hpp>
#include <diagnostic_msgs/srv/self_test.hpp>
#include <diagnostic_msgs/srv/add_diagnostics.hpp>

//test_msgs
#include <test_msgs/msg/empty.hpp>
#include <test_msgs/msg/arrays.hpp>
#include <test_msgs/msg/nested.hpp>
#include <test_msgs/msg/strings.hpp>
#include <test_msgs/msg/builtins.hpp>
#include <test_msgs/msg/defaults.hpp>
#include <test_msgs/msg/constants.hpp>
#include <test_msgs/msg/w_strings.hpp>
#include <test_msgs/msg/basic_types.hpp>
#include <test_msgs/msg/multi_nested.hpp>
#include <test_msgs/msg/bounded_sequences.hpp>
#include <test_msgs/msg/unbounded_sequences.hpp>
#include <test_msgs/srv/empty.hpp>
#include <test_msgs/srv/arrays.hpp>
#include <test_msgs/srv/basic_types.hpp>
#include <test_msgs/action/fibonacci.hpp>
#include <test_msgs/action/nested_message.hpp>

//action_msgs
#include <action_msgs/msg/goal_info.hpp>
#include <action_msgs/msg/goal_status.hpp>
#include <action_msgs/msg/goal_status_array.hpp>
#include <action_msgs/srv/cancel_goal.hpp>

//actionlib_msgs
#include <actionlib_msgs/msg/goal_id.hpp>
#include <actionlib_msgs/msg/goal_status.hpp>
#include <actionlib_msgs/msg/goal_status_array.hpp>

//lifecycle_msgs
#include <lifecycle_msgs/msg/state.hpp>
#include <lifecycle_msgs/msg/transition.hpp>
#include <lifecycle_msgs/msg/transition_event.hpp>
#include <lifecycle_msgs/msg/transition_description.hpp>
#include <lifecycle_msgs/srv/get_state.hpp>
#include <lifecycle_msgs/srv/change_state.hpp>
#include <lifecycle_msgs/srv/get_available_states.hpp>
#include <lifecycle_msgs/srv/get_available_transitions.hpp>

//rosgraph_msgs
#include <rosgraph_msgs/msg/clock.hpp>

//pendulum_msgs
#include <pendulum_msgs/msg/joint_state.hpp>
#include <pendulum_msgs/msg/joint_command.hpp>
#include <pendulum_msgs/msg/rttest_results.hpp>

//composition_interfaces
#include <composition_interfaces/srv/load_node.hpp>
#include <composition_interfaces/srv/list_nodes.hpp>
#include <composition_interfaces/srv/unload_node.hpp>

//unique_identifier_msgs
#include <unique_identifier_msgs/msg/uuid.hpp>

//tf2_msgs
#include <tf2_msgs/msg/tf2_error.hpp>
#include <tf2_msgs/msg/tf_message.hpp>
#include <tf2_msgs/srv/frame_graph.hpp>
#include <tf2_msgs/action/lookup_transform.hpp>

//tf2_sensor_msgs

//tf2_geometry_msgs

#endif
View Code

　　以下是详细代码，依赖于C++14、OpenCV4.x和Spdlog。

  1 #include <opencv2/opencv.hpp>
  2 #include <opencv2/core/utils/filesystem.hpp>
  3 #include <spdlog/spdlog.h>
  4 using namespace std;
  5 using namespace cv;
  6 
  7 class ROS2WebotsHeaders
  8 {
  9 public:
 10     static void TestMe(int argc, char** argv)
 11     {
 12         if (argc < 3) spdlog::critical("Usage: appName whichLib(ros or webots) homePath(/usr/local/webots or /opt/ros/foxy)");
 13         else spdlog::info("Save the result to {}/abcx.xxx", argv[2]);
 14         if (argc >= 3 && strcmp(argv[1], "ros") == 0) createROSHPP(argv[2]);
 15         if (argc >= 3 && strcmp(argv[1], "webots") == 0) createWebotHPP(argv[2]);
 16     }
 17 
 18 public:
 19     static void createWebotHPP(string webotsHomePath = "/user/local/webots")
 20     {
 21         string hDir = webotsHomePath + "/include/controller/c/webots";
 22         string hppDir = webotsHomePath + "/include/controller/cpp/webots";
 23         vector<string> webotsHs = globPaths(hDir, "*.h", 0, true);
 24         vector<string> webotsHpps = globPaths(hppDir, "*.hpp", 0, true);
 25         FILE* file = fopen(getNewPath(webotsHomePath, "abc", "hpp", -1).c_str(), "w");
 26         //for (size_t k = 0; k < webotsHs.size(); ++k) fprintf(file, "#include <webots%s>
", webotsHs[k].substr(hDir.size()).c_str()); //C
 27         for (size_t k = 0; k < webotsHpps.size(); ++k) fprintf(file, "#include <webots%s>
", webotsHpps[k].substr(hppDir.size()).c_str());//CPP
 28         fclose(file);
 29     }
 30     static void createROSHPP(string rosHomePath = "/opt/ros/foxy/inclue")
 31     {
 32         //0.
 33         auto writeROSHPP = [](string modDir, int choice/*1=cur 2=msg 4=srv 8=action*/, FILE* file)->void
 34         {
 35             string modName = modDir.substr(modDir.find_last_of('/') + 1);
 36             string msgDir = modDir + "/msg";
 37             string srvDir = modDir + "/srv";
 38             string actionDir = modDir + "/action";
 39             vector<string> selfHeaders = cv::utils::fs::exists(modDir) && choice & 1 ? globPaths(modDir, "*.hpp", 0, false) : vector<string>();
 40             vector<string> msgHeaders = cv::utils::fs::exists(msgDir) && choice & 2 ? globPaths(msgDir, "*.hpp", 0, false) : vector<string>();
 41             vector<string> srvHeaders = cv::utils::fs::exists(srvDir) && choice & 4 ? globPaths(srvDir, "*.hpp", 0, false) : vector<string>();
 42             vector<string> actionHeaders = cv::utils::fs::exists(actionDir) && choice & 8 ? globPaths(actionDir, "*.hpp", 0, false) : vector<string>();
 43             vector<string> allHeaders = { "
//" + modName + "
" };
 44             for (size_t k = 0; k < selfHeaders.size(); ++k)
 45                 if (selfHeaders[k].find("__") == string::npos && selfHeaders[k].find("impl") == string::npos)
 46                     allHeaders.push_back("#include <" + modName + selfHeaders[k].substr(selfHeaders[k].find_last_of('/')) + ">
");
 47             for (size_t k = 0; k < msgHeaders.size(); ++k)
 48                 if (msgHeaders[k].find("__") == string::npos && msgHeaders[k].find("impl") == string::npos)
 49                     allHeaders.push_back("#include <" + modName + "/msg" + msgHeaders[k].substr(msgHeaders[k].find_last_of('/')) + ">
");
 50             for (size_t k = 0; k < srvHeaders.size(); ++k)
 51                 if (srvHeaders[k].find("__") == string::npos && srvHeaders[k].find("impl") == string::npos)
 52                     allHeaders.push_back("#include <" + modName + "/srv" + srvHeaders[k].substr(srvHeaders[k].find_last_of('/')) + ">
");
 53             for (size_t k = 0; k < actionHeaders.size(); ++k)
 54                 if (actionHeaders[k].find("__") == string::npos && actionHeaders[k].find("impl") == string::npos)
 55                     allHeaders.push_back("#include <" + modName + "/action" + actionHeaders[k].substr(actionHeaders[k].find_last_of('/')) + ">
");
 56             if (file != nullptr) for (size_t k = 0; k < allHeaders.size(); ++k) fprintf(file, allHeaders[k].c_str());
 57             //return allHeaders;
 58         };
 59         FILE* file = fopen(getNewPath(rosHomePath, "abc", "hpp", -1).c_str(), "w");
 60         rosHomePath += "/include";
 61 
 62         //1:Cores
 63         writeROSHPP(rosHomePath + "/ament_index_cpp", 1, file); //depend on env AMENT_PREFIX_PATH
 64         writeROSHPP(rosHomePath + "/rclcpp", 1, file);
 65         writeROSHPP(rosHomePath + "/rclcpp_action", 1, file);
 66 
 67         //2:Standards
 68         writeROSHPP(rosHomePath + "/std_msgs", 14, file);
 69         writeROSHPP(rosHomePath + "/std_srvs", 14, file);
 70         writeROSHPP(rosHomePath + "/builtin_interfaces", 14, file);
 71         writeROSHPP(rosHomePath + "/sensor_msgs", 15, file);
 72         writeROSHPP(rosHomePath + "/geometry_msgs", 14, file);
 73         writeROSHPP(rosHomePath + "/trajectory_msgs", 14, file);
 74         writeROSHPP(rosHomePath + "/move_base_msgs", 14, file);
 75         writeROSHPP(rosHomePath + "/stereo_msgs", 14, file);
 76         writeROSHPP(rosHomePath + "/shape_msgs", 14, file);
 77         writeROSHPP(rosHomePath + "/map_msgs", 14, file);
 78         writeROSHPP(rosHomePath + "/nav_msgs", 14, file);
 79         writeROSHPP(rosHomePath + "/visualization_msgs", 14, file);
 80 
 81         //3.Introspections
 82         writeROSHPP(rosHomePath + "/diagnostic_msgs", 14, file);
 83         writeROSHPP(rosHomePath + "/test_msgs", 14, file);
 84         writeROSHPP(rosHomePath + "/action_msgs", 14, file);
 85         writeROSHPP(rosHomePath + "/actionlib_msgs", 14, file);
 86         writeROSHPP(rosHomePath + "/lifecycle_msgs", 14, file);
 87         writeROSHPP(rosHomePath + "/rosgraph_msgs", 14, file);
 88         writeROSHPP(rosHomePath + "/pendulum_msgs", 14, file);
 89         writeROSHPP(rosHomePath + "/composition_interfaces", 14, file);
 90         writeROSHPP(rosHomePath + "/unique_identifier_msgs", 14, file);
 91 
 92         //4:SalientFields: TF2+URDF
 93         writeROSHPP(rosHomePath + "/tf2_msgs", 14, file);
 94         writeROSHPP(rosHomePath + "/tf2_sensor_msgs", 14, file);
 95         writeROSHPP(rosHomePath + "/tf2_geometry_msgs", 14, file);
 96 
 97         //
 98         fclose(file);
 99     }
100 
101 public:
102     static vector<string> globPaths(string dir, string pattern = "*", int type = 0/*0=files 1=dirs 2=all*/, bool recursive = false)
103     {
104         if (cv::utils::fs::exists(dir) == false) return vector<string>();
105 
106         //1.Glob all paths including directories and files
107         vector<string> srcPaths;
108         cv::utils::fs::glob(dir, pattern, srcPaths, recursive, true);
109         for (uint64 k = 0, pos = 0; k < srcPaths.size(); ++k)
110             for (uint64 ind = 0; ; ++ind)
111             {
112                 ind = srcPaths[k].find('\', ind);
113                 if (ind != string::npos) srcPaths[k].replace(ind, 1, "/");
114                 else break;
115             }
116 
117         //2.Separate all paths into directories and files
118         vector<string> dirPaths, filePaths;
119         for (uint64 k = 0; k < srcPaths.size(); ++k) cv::utils::fs::isDirectory(srcPaths[k]) ? dirPaths.push_back(srcPaths[k]) : filePaths.push_back(srcPaths[k]);
120 
121         //3.Sort dirPaths and filePaths separately
122         if (dirPaths.size() > 1) stable_sort(dirPaths.begin(), dirPaths.end(), less<string>());
123         if (dirPaths.size() > 1) stable_sort(dirPaths.begin(), dirPaths.end(), [](string str1, string str2)->bool {return str1.length() < str2.length(); });
124         if (filePaths.size() > 1) stable_sort(filePaths.begin(), filePaths.end(), less<string>());
125         if (filePaths.size() > 1) stable_sort(filePaths.begin(), filePaths.end(), [](string str1, string str2)->bool {return str1.length() < str2.length(); });
126 
127         //4.Return specified paths
128         if (type == 0) return filePaths;
129         if (type == 1) return dirPaths;
130         if (type == 2) for (uint k = 0; k < dirPaths.size(); ++k) filePaths.push_back(dirPaths[k]);
131         return filePaths;
132     }
133     static string getNewPath(string dir, string basename, string extname = ""/*empty for dirPath*/, int64 initialId = -1/*refer to comments*/)
134     {
135         if (initialId >= 0)//e.g: if file0 file3 file5 file7 file9 exist, return file1 for initialId=0, file4 for initialId=3, file10 for initialId < = 0
136         {
137             string fullPath;
138             do
139             {
140                 fullPath = fmt::format("{}/{}{}{}", dir, basename, initialId, (extname.empty() ? extname : "." + extname));
141                 if (utils::fs::exists(fullPath) == false) break;
142             } while (++initialId);
143             return fullPath;
144         }
145         else
146         {
147             vector<string> paths = globPaths(dir, basename + "*", extname.empty() ? 1 : 0, false);
148             if (paths.empty()) return fmt::format("{}/{}{}{}", dir, basename, 0, (extname.empty() ? extname : "." + extname));
149             if (extname.empty())
150             {
151                 int64 ind = paths[paths.size() - 1].find(basename) + basename.length();
152                 int64 id = atoll(paths[paths.size() - 1].substr(ind).c_str()) + 1;
153                 return fmt::format("{}/{}{}", dir, basename, id);
154             }
155             else
156             {
157                 int64 ind1 = paths[paths.size() - 1].find(basename) + basename.length();
158                 int64 ind2 = paths[paths.size() - 1].find_last_of(".");
159                 int64 id = atoll(paths[paths.size() - 1].substr(ind1, ind2 - ind1).c_str()) + 1;
160                 return fmt::format("{}/{}{}.{}", dir, basename, id, extname);
161             }
162         }
163     }
164 };
165 
166 int main(int argc, char** argv){ ROS2WebotsHeaders::TestMe(argc, argv); return 0; }
View Code
【推广】 免费学中医，健康全家人
原文地址：https://www.cnblogs.com/dzyBK/p/13818212.html