├── LICENSE ├── ReadMe.md ├── cyberdog_example ├── CMakeLists.txt ├── include │ └── cyberdog_example │ │ └── gamepad_lcmt.hpp ├── package.xml └── src │ ├── cyberdogmsg_sender.cpp │ └── keybroad_commander.cpp ├── cyberdog_gazebo ├── CMakeLists.txt ├── config.h.cmake ├── include │ ├── actuator.hpp │ ├── ctrl_ros │ │ ├── c_types.h │ │ ├── control_parameters │ │ │ ├── control_parameter_interface.hpp │ │ │ ├── control_parameters.hpp │ │ │ └── robot_parameters.hpp │ │ ├── cpp_types.hpp │ │ ├── parameters.hpp │ │ ├── sim_utilities │ │ │ ├── gamepad_command.hpp │ │ │ ├── gamepad_lcmt.hpp │ │ │ ├── imu_types.hpp │ │ │ ├── simulator_message.hpp │ │ │ ├── spine_board.hpp │ │ │ └── visualization_data.hpp │ │ └── utilities │ │ │ ├── shared_memory.hpp │ │ │ └── utilities.h │ ├── cyberdog_sync_lcmt.hpp │ ├── foot_contact_plugin.hpp │ ├── gamepad_lcmt.hpp │ ├── gazebo_foot_contact.hpp │ ├── lcmhandler.hpp │ ├── legged_plugin.hpp │ ├── legged_simparam.hpp │ ├── node_executor.hpp │ └── simulator_lcmt.hpp ├── launch │ ├── cyberdog_control_launch.py │ ├── gazebo.launch.py │ ├── gazebo_nav.launch.py │ └── heightmap_gazebo.launch.py ├── media │ └── materials │ │ ├── CMakeLists.txt │ │ └── textures │ │ ├── CMakeLists.txt │ │ ├── WoodPallet.png │ │ ├── beigeWall.jpg │ │ ├── ceiling_tiled.jpg │ │ ├── clouds.jpg │ │ ├── dirt_diffusespecular.png │ │ ├── dirt_normal.png │ │ ├── flat_normal.png │ │ ├── fungus_diffusespecular.png │ │ ├── fungus_normal.png │ │ ├── granite.jpg │ │ ├── granite2.jpg │ │ ├── grass.jpg │ │ ├── grass_diffusespecular.png │ │ ├── grass_normal.png │ │ ├── hardwood_floor.jpg │ │ ├── motorway.jpg │ │ ├── paintedWall.jpg │ │ ├── pioneerBody.jpg │ │ ├── primary.jpg │ │ ├── projection_filter.png │ │ ├── random.png │ │ ├── residential.jpg │ │ ├── secondary.jpg │ │ ├── sidewalk.jpg │ │ ├── steps.jpeg │ │ ├── stereo_projection_pattern_high_res.png │ │ ├── stereo_projection_pattern_high_res_red.png │ │ ├── terrain.png │ │ ├── texture_information.txt │ │ ├── trunk.jpg │ │ ├── white.bmp │ │ ├── willowMap.png │ │ └── wood.jpg ├── package.xml ├── script │ ├── gazebolauncher.py │ ├── launchcontrol.sh │ ├── launchgazebo.sh │ ├── launchsim.py │ └── launchvisual.sh ├── src │ ├── control_parameters │ │ ├── control_parameter_interface.cpp │ │ └── control_parameters.cpp │ ├── foot_contact_plugin.cpp │ ├── lcmhandler.cpp │ ├── legged_plugin.cpp │ ├── legged_simparam.cpp │ ├── sim_utilities │ │ └── spine_board.cpp │ └── utilities │ │ └── utilities.cpp ├── third-party │ └── ParamHandler │ │ ├── CMakeLists.txt │ │ ├── LICENSE.txt │ │ ├── ParamHandler.cpp │ │ ├── ParamHandler.hpp │ │ ├── README.md │ │ ├── param_helper.hpp │ │ └── src │ │ ├── binary.cpp │ │ ├── collectionstack.h │ │ ├── contrib │ │ ├── graphbuilder.cpp │ │ ├── graphbuilderadapter.cpp │ │ └── graphbuilderadapter.h │ │ ├── convert.cpp │ │ ├── directives.cpp │ │ ├── directives.h │ │ ├── emit.cpp │ │ ├── emitfromevents.cpp │ │ ├── emitter.cpp │ │ ├── emitterstate.cpp │ │ ├── emitterstate.h │ │ ├── emitterutils.cpp │ │ ├── emitterutils.h │ │ ├── exceptions.cpp │ │ ├── exp.cpp │ │ ├── exp.h │ │ ├── indentation.h │ │ ├── memory.cpp │ │ ├── node.cpp │ │ ├── node_data.cpp │ │ ├── nodebuilder.cpp │ │ ├── nodebuilder.h │ │ ├── nodeevents.cpp │ │ ├── nodeevents.h │ │ ├── null.cpp │ │ ├── ostream_wrapper.cpp │ │ ├── parse.cpp │ │ ├── parser.cpp │ │ ├── ptr_vector.h │ │ ├── regex_yaml.cpp │ │ ├── regex_yaml.h │ │ ├── regeximpl.h │ │ ├── scanner.cpp │ │ ├── scanner.h │ │ ├── scanscalar.cpp │ │ ├── scanscalar.h │ │ ├── scantag.cpp │ │ ├── scantag.h │ │ ├── scantoken.cpp │ │ ├── setting.h │ │ ├── simplekey.cpp │ │ ├── singledocparser.cpp │ │ ├── singledocparser.h │ │ ├── stream.cpp │ │ ├── stream.h │ │ ├── streamcharsource.h │ │ ├── stringsource.h │ │ ├── tag.cpp │ │ ├── tag.h │ │ └── token.h └── world │ ├── heightmap.world │ └── simple.world ├── cyberdog_msg ├── CMakeLists.txt ├── msg │ ├── ApplyForce.msg │ └── YamlParam.msg └── package.xml ├── cyberdog_robot └── cyberdog_description │ ├── CMakeLists.txt │ ├── launch │ ├── cyberdog.launch.py │ └── cyberdog_description.launch.py │ ├── meshes │ ├── abad.dae │ ├── body.dae │ ├── hip.dae │ ├── hip_mirror.dae │ └── knee.dae │ ├── package.xml │ ├── rviz │ └── cyberdog.rviz │ └── xacro │ ├── const.xacro │ ├── gazebo.xacro │ ├── leg.xacro │ ├── robot.xacro │ └── transmission.xacro ├── cyberdog_simulator ├── CMakeLists.txt └── package.xml └── cyberdog_visual ├── CMakeLists.txt ├── include └── cyberdog_visual │ └── cyberdog_visual.hpp ├── launch ├── cyberdog_lcm_repaly.launch.py └── cyberdog_visual.launch.py ├── lcm_type ├── leg_control_data_lcmt.hpp ├── localization_lcmt.hpp ├── simulator_lcmt.hpp └── state_estimator_lcmt.hpp ├── package.xml ├── rviz ├── config.rviz ├── cyberdog_visual2.rviz └── cyberdog_visual_replay.rviz └── src ├── cyberdog_visual.cpp └── cyberdog_visual_node.cpp /ReadMe.md: -------------------------------------------------------------------------------- 1 | # CyberDog simulator 2 | 3 | 该仿真平台使用gazebo plugin的形式，消除ros通信不同步对控制的影响。同时，提供了基于Rviz2的可视化工具，将机器人状态的lcm数据转发到ROS。 4 | 该仿真平台需和cyberdog_locomotion仓通信进行使用，建议通过cyberdog_sim仓进行安装 5 | 6 | 详细信息可参照[**仿真平台文档**](https://miroboticslab.github.io/blogs/#/cn/cyberdog_gazebo_cn) 7 | 8 | 推荐安装环境： Ubuntu 20.04 + ROS2 Galactic 9 | 10 | ## 依赖安装 11 | 运行仿真平台需要安装如下的依赖 12 | **ros2_Galactic** 13 | ``` 14 | $ sudo apt update && sudo apt install curl gnupg lsb-release 15 | $ sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg 16 | $ echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(source /etc/os-release && echo $UBUNTU_CODENAME) main" | sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null 17 | $ sudo apt update 18 | $ sudo apt install ros-galactic-desktop 19 | ``` 20 | **Gazebo** 21 | ``` 22 | $ curl -sSL http://get.gazebosim.org | sh 23 | $ sudo apt install ros-galactic-gazebo-ros 24 | $ sudo apt install ros-galactic-gazebo-msgs 25 | ``` 26 | **LCM** 27 | ``` 28 | $ git clone https://github.com/lcm-proj/lcm.git 29 | $ cd lcm 30 | $ mkdir build 31 | $ cd build 32 | $ cmake -DLCM_ENABLE_JAVA=ON .. 33 | $ make 34 | $ sudo make install 35 | ``` 36 | **Eigen** 37 | ``` 38 | $ git clone https://github.com/eigenteam/eigen-git-mirror 39 | $ cd eigen-git-mirror 40 | $ mkdir build 41 | $ cd build 42 | $ cmake .. 43 | $ sudo make install 44 | ``` 45 | **xacro** 46 | ``` 47 | $ sudo apt install ros-galactic-xacro 48 | ``` 49 | 50 | **vcstool** 51 | ``` 52 | $ sudo apt install python3-vcstool 53 | ``` 54 | 55 | **colcon** 56 | ``` 57 | $ sudo apt install python3-colcon-common-extensions 58 | ``` 59 | 60 | 注意：若环境中安装有其他版本的yaml-cpp，可能会与ros galactic 自带的yaml-cpp发生冲突，建议编译时环境中无其他版本yaml-cpp 61 | 62 | ## 下载 63 | ``` 64 | $ git clone https://github.com/MiRoboticsLab/cyberdog_sim.git 65 | $ cd cyberdog_sim 66 | $ vcs import < cyberdog_sim.repos 67 | ``` 68 | ## 编译 69 | 需要将src/cyberdog locomotion/CMakeLists.txt中的BUILD_ROS置为ON 70 | 然后需要在cyberdog_sim文件夹下进行编译 71 | ``` 72 | $ source /opt/ros/galactic/setup.bash 73 | $ colcon build --merge-install --symlink-install --packages-up-to cyberdog_locomotion cyberdog_simulator 74 | ``` 75 | 76 | ## 使用 77 | 需要在cyberdog_sim文件夹下运行 78 | ``` 79 | $ python3 src/cyberdog_simulator/cyberdog_gazebo/script/launchsim.py 80 | ``` 81 | 82 | ### 也可以通过以下命令分别运行各程序： 83 | 84 | 首先启动gazebo程序，于cyberdog_sim文件夹下进行如下操作： 85 | ``` 86 | $ source /opt/ros/galactic/setup.bash 87 | $ source install/setup.bash 88 | $ ros2 launch cyberdog_gazebo gazebo.launch.py 89 | ``` 90 | 也可通过如下命令打开激光雷达 91 | ``` 92 | $ source /opt/ros/galactic/setup.bash 93 | $ source install/setup.bash 94 | $ ros2 launch cyberdog_gazebo gazebo.launch.py use_lidar:=true 95 | ``` 96 | 97 | 然后启动cyberdog_locomotion仓的控制程序。在cyberdog_sim文件夹下运行： 98 | ``` 99 | $ source /opt/ros/galactic/setup.bash 100 | $ source install/setup.bash 101 | $ ros2 launch cyberdog_gazebo cyberdog_control_launch.py 102 | ``` 103 | 104 | 最后打开可视化界面，在cyberdog_sim文件夹下运行： 105 | ``` 106 | $ source /opt/ros/galactic/setup.bash 107 | $ source install/setup.bash 108 | $ ros2 launch cyberdog_visual cyberdog_visual.launch.py 109 | ``` 110 | 111 | ### 播放实机lcm log数据 112 | 该平台能够通过rviz2将实机的运动控制lcm数据进行播放和可视化。 113 | 操作方法如下： 114 | 运行lcm数据可视化界面，于cyberdog_sim文件夹下进行如下操作： 115 | ``` 116 | $ source /opt/ros/galactic/setup.bash 117 | $ source install/setup.bash 118 | $ ros2 launch cyberdog_visual cyberdog_lcm_repaly.launch.py 119 | ``` 120 | 打开可视化界面后，通过运行cyberdog_locomotion仓的script目录下的脚本播放lcm log数据。 121 | 于cyberdog_sim文件夹下进行如下操作： 122 | ``` 123 | $ cd src/cyberdog_locomotion/scripts 124 | $ ./make_types.sh #初次使用时需要运行 125 | $ ./launch_lcm_logplayer.sh 126 | ``` 127 | 运行后选择需要播放的lcm log文件，即可进行log数据的播放，此时通过rviz可视化界面能复现机器人的姿态。 128 | -------------------------------------------------------------------------------- /cyberdog_example/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(cyberdog_example) 3 | 4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 5 | add_compile_options(-Wall -Wextra -Wpedantic) 6 | endif() 7 | 8 | # find dependencies 9 | find_package(ament_cmake REQUIRED) 10 | find_package(rclcpp REQUIRED) 11 | find_package(rclcpp_action REQUIRED) 12 | find_package(std_msgs REQUIRED) 13 | find_package(cyberdog_msg REQUIRED) 14 | find_package(lcm REQUIRED) 15 | 16 | include_directories(include) 17 | 18 | add_executable(cyberdogmsg_sender src/cyberdogmsg_sender.cpp) 19 | ament_target_dependencies(cyberdogmsg_sender rclcpp rclcpp_action cyberdog_msg) 20 | 21 | add_executable(keybroad_commander src/keybroad_commander.cpp) 22 | ament_target_dependencies(keybroad_commander rclcpp rclcpp_action cyberdog_msg) 23 | target_link_libraries(keybroad_commander lcm) 24 | 25 | ament_package() 26 | -------------------------------------------------------------------------------- /cyberdog_example/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | cyberdog_example 5 | 0.0.0 6 | TODO: Package description 7 | ljh 8 | TODO: License declaration 9 | 10 | ament_cmake 11 | 12 | ament_lint_auto 13 | ament_lint_common 14 | 15 | cyberdog_msg 16 | 17 | std_msgs 18 | rclcpp 19 | rclcpp_action 20 | 21 | 22 | ament_cmake 23 | 24 | 25 | -------------------------------------------------------------------------------- /cyberdog_example/src/cyberdogmsg_sender.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | enum class ControlParameterValueKind : uint64_t { 8 | kDOUBLE = 1, 9 | kS64 = 2, 10 | kVEC_X_DOUBLE = 3, // for template type derivation 11 | kMAT_X_DOUBLE = 4 // for template type derivation 12 | }; 13 | 14 | class ExampleNode: public rclcpp::Node 15 | { 16 | public: 17 | ExampleNode(std::string node_name):Node(node_name){}; 18 | ~ExampleNode(){}; 19 | }; 20 | 21 | int main(int argc, char** argv){ 22 | rclcpp::init(argc, argv); 23 | auto example_node_=std::make_shared("cyberdogmsg_node"); 24 | rclcpp::Publisher::SharedPtr para_pub_; 25 | rclcpp::Publisher::SharedPtr force_pub_; 26 | 27 | para_pub_=example_node_->create_publisher("yaml_parameter",10); 28 | force_pub_=example_node_->create_publisher("apply_force",10); 29 | 30 | auto param_message_ = cyberdog_msg::msg::YamlParam(); 31 | auto force_message_ = cyberdog_msg::msg::ApplyForce(); 32 | 33 | sleep(1); 34 | 35 | //send control mode to Gamepad_controller mode which can enable control from simulator program 36 | param_message_.name = "use_rc"; 37 | param_message_.kind = uint64_t(ControlParameterValueKind::kS64); 38 | param_message_.s64_value = int64_t(0); 39 | param_message_.is_user = 0; 40 | para_pub_->publish(param_message_); 41 | std::cout<<"switch to gamepad control model..."<publish(param_message_); 52 | std::cout<<"recovery stand ..."<publish(param_message_); 62 | std::cout<<"locomotion ..."< rel_pos_; 68 | std::array force_; 69 | force_[2] = 20; 70 | 71 | force_message_.link_name = "FL_knee"; 72 | force_message_.rel_pos = rel_pos_; 73 | force_message_.force = force_; 74 | force_message_.time = 2; 75 | force_pub_->publish(force_message_); 76 | std::cout<<"applying force on FL_knee ..."< vecxd_value_; 82 | vecxd_value_[0] = 0.2; 83 | vecxd_value_[2] = 0.25; 84 | 85 | param_message_.name = "des_roll_pitch_height"; 86 | param_message_.kind = uint64_t(ControlParameterValueKind::kVEC_X_DOUBLE); 87 | param_message_.vecxd_value = vecxd_value_; 88 | param_message_.is_user = 1; 89 | para_pub_->publish(param_message_); 90 | std::cout<<"set roll angle to 0.2 ..."<publish(param_message_); 101 | std::cout<<"recovery stand ..."< 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include "cyberdog_example/gamepad_lcmt.hpp" 8 | 9 | enum class ControlParameterValueKind : uint64_t { 10 | kDOUBLE = 1, 11 | kS64 = 2, 12 | kVEC_X_DOUBLE = 3, // for template type derivation 13 | kMAT_X_DOUBLE = 4 // for template type derivation 14 | }; 15 | 16 | class ExampleNode: public rclcpp::Node 17 | { 18 | public: 19 | ExampleNode(std::string node_name):Node(node_name){}; 20 | ~ExampleNode(){}; 21 | }; 22 | 23 | int main(int argc, char** argv){ 24 | //initialize rclcpp 25 | rclcpp::init(argc, argv); 26 | auto example_node_=std::make_shared("kdcommand_node"); 27 | rclcpp::Publisher::SharedPtr para_pub_; 28 | para_pub_=example_node_->create_publisher("yaml_parameter",10); 29 | 30 | auto param_message_ = cyberdog_msg::msg::YamlParam(); 31 | 32 | sleep(1); 33 | 34 | //send control mode to Gamepad_controller mode which can enable control from simulator program 35 | param_message_.name = "use_rc"; 36 | param_message_.kind = uint64_t(ControlParameterValueKind::kS64); 37 | param_message_.s64_value = int64_t(0); 38 | param_message_.is_user = 0; 39 | para_pub_->publish(param_message_); 40 | std::cout<<"switch to gamepad control model..."<> key; 56 | switch(key){ 57 | case 'w': //x direction speed 58 | if(gamepad_lcm_type.leftStickAnalog[1]<0.9) 59 | gamepad_lcm_type.leftStickAnalog[1] += 0.1; 60 | break; 61 | case 's': //x direction speed 62 | if(gamepad_lcm_type.leftStickAnalog[1]>-0.9) 63 | gamepad_lcm_type.leftStickAnalog[1] += -0.1; 64 | break; 65 | case 'd': //y direction speed 66 | if(gamepad_lcm_type.leftStickAnalog[0]<0.9) 67 | gamepad_lcm_type.leftStickAnalog[0] += 0.1; 68 | break; 69 | case 'a': //y direction speed 70 | if(gamepad_lcm_type.leftStickAnalog[0]<0.9) 71 | gamepad_lcm_type.leftStickAnalog[0] += -0.1; 72 | break; 73 | case 'i': //pitch direction speed 74 | if(gamepad_lcm_type.rightStickAnalog[1]<0.9) 75 | gamepad_lcm_type.rightStickAnalog[1] += 0.1; 76 | break; 77 | case 'k': //pitch direction speed 78 | if(gamepad_lcm_type.rightStickAnalog[1]>-0.9) 79 | gamepad_lcm_type.rightStickAnalog[1] += -0.1; 80 | break; 81 | case 'j': //yaw direction speed 82 | if(gamepad_lcm_type.rightStickAnalog[0]<0.9) 83 | gamepad_lcm_type.rightStickAnalog[0] += 0.1; 84 | break; 85 | case 'l': //yaw direction speed 86 | if(gamepad_lcm_type.rightStickAnalog[0]>-0.9) 87 | gamepad_lcm_type.rightStickAnalog[0] += -0.1; 88 | break; 89 | case 'e': //QP stand 90 | gamepad_lcm_type.x = 1; 91 | break; 92 | case 'r': //locomotion 93 | gamepad_lcm_type.y = 1; 94 | break; 95 | case 't': //pure damper 96 | gamepad_lcm_type.a = 1; 97 | break; 98 | case 'y': //recoverystand 99 | gamepad_lcm_type.b = 1; 100 | break; 101 | case 'c': //clear speed of all direction 102 | gamepad_lcm_type.leftStickAnalog[0] = 0; 103 | gamepad_lcm_type.leftStickAnalog[1] = 0; 104 | gamepad_lcm_type.rightStickAnalog[0] = 0; 105 | gamepad_lcm_type.rightStickAnalog[1] = 0; 106 | break; 107 | } 108 | 109 | //publish lcm 110 | lcm_.publish("gamepad_lcmt",&gamepad_lcm_type); 111 | 112 | //clear button states 113 | gamepad_lcm_type.x = 0; 114 | gamepad_lcm_type.y = 0; 115 | gamepad_lcm_type.a = 0; 116 | gamepad_lcm_type.b = 0; 117 | } 118 | 119 | 120 | } -------------------------------------------------------------------------------- /cyberdog_gazebo/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(cyberdog_gazebo) 3 | 4 | set(CMAKE_IGNORE_PATH "/usr/local") 5 | 6 | # Default to C99 7 | if(NOT CMAKE_C_STANDARD) 8 | set(CMAKE_C_STANDARD 99) 9 | endif() 10 | 11 | # Default to C++14 12 | if(NOT CMAKE_CXX_STANDARD) 13 | set(CMAKE_CXX_STANDARD 14) 14 | endif() 15 | 16 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 17 | add_compile_options(-Wall -Wextra -Wpedantic) 18 | endif() 19 | 20 | if(ONBOARD_BUILD) 21 | SET (THIS_COM "../" ) 22 | CONFIGURE_FILE(${CMAKE_CURRENT_SOURCE_DIR}/config.h.cmake 23 | ${CMAKE_BINARY_DIR}/Configuration.h) 24 | else(ONBOARD_BUILD) 25 | SET (THIS_COM "${PROJECT_SOURCE_DIR}/" ) 26 | CONFIGURE_FILE(${CMAKE_CURRENT_SOURCE_DIR}/config.h.cmake 27 | ${CMAKE_BINARY_DIR}/Configuration.h) 28 | endif(ONBOARD_BUILD) 29 | 30 | # find dependencies 31 | find_package(ament_cmake REQUIRED) 32 | find_package(rosidl_default_generators REQUIRED) 33 | find_package(rosidl_typesupport_cpp REQUIRED) 34 | find_package(rclcpp REQUIRED) 35 | find_package(rclcpp_components REQUIRED) 36 | find_package(Eigen3 REQUIRED) 37 | find_package(gazebo_dev REQUIRED) 38 | find_package(gazebo_ros REQUIRED) 39 | find_package(gazebo_msgs REQUIRED) 40 | find_package(cyberdog_msg REQUIRED) 41 | find_package(lcm REQUIRED) 42 | 43 | 44 | set(dependencies 45 | rclcpp 46 | Eigen3 47 | gazebo_msgs 48 | gazebo_dev 49 | gazebo_ros 50 | cyberdog_msg 51 | ) 52 | 53 | file(GLOB_RECURSE sources "src/control_parameters/*.cpp" 54 | "src/sim_utilities/*.cpp" 55 | "src/utilities/*.cpp") 56 | 57 | include_directories(include) 58 | include_directories(include/ctrl_ros) 59 | include_directories("third-party/ParamHandler") 60 | include_directories(${CMAKE_BINARY_DIR}) 61 | 62 | add_subdirectory("third-party/ParamHandler") 63 | 64 | add_library(foot_contact_plugin SHARED src/foot_contact_plugin.cpp) 65 | ament_target_dependencies(foot_contact_plugin ${dependencies}) 66 | target_link_libraries(foot_contact_plugin ${GAZEBO_LIBRARIES} lcm) 67 | 68 | add_library(legged_plugin SHARED ${sources} src/legged_plugin.cpp src/legged_simparam.cpp src/lcmhandler.cpp) 69 | ament_target_dependencies(legged_plugin ${dependencies}) 70 | target_link_libraries(legged_plugin ${GAZEBO_LIBRARIES} param_handler pthread lcm) 71 | 72 | install(TARGETS legged_plugin param_handler foot_contact_plugin 73 | EXPORT export_${PROJECT_NAME} 74 | ARCHIVE DESTINATION lib 75 | LIBRARY DESTINATION lib 76 | RUNTIME DESTINATION bin 77 | ) 78 | 79 | # Mark other files for installation 80 | install( 81 | DIRECTORY 82 | launch 83 | world 84 | DESTINATION share/${PROJECT_NAME} 85 | ) 86 | 87 | ament_package() 88 | -------------------------------------------------------------------------------- /cyberdog_gazebo/config.h.cmake: -------------------------------------------------------------------------------- 1 | #ifndef CTRL_CONFIG_H 2 | #define CTRL_CONFIG_H 3 | 4 | #define THIS_COM "@THIS_COM@" 5 | 6 | #endif 7 | -------------------------------------------------------------------------------- /cyberdog_gazebo/include/ctrl_ros/c_types.h: -------------------------------------------------------------------------------- 1 | /*! @file c_types.h 2 | * @brief Common types that can also be included in C code 3 | * 4 | * This file contains types which are shared between C and C++ code. The 5 | * low-level drivers (rt folder) are all in C and everything else is C++. 6 | * Because this file is included in both C and C++, it can't contain C++ type 7 | * alias ("using"), namespaces, or templates. 8 | */ 9 | 10 | #ifndef PROJECT_CTYPES_H 11 | #define PROJECT_CTYPES_H 12 | 13 | #include // for size_t 14 | #include 15 | 16 | // short version of the stdint default integer types 17 | typedef uint64_t u64; 18 | typedef uint32_t u32; 19 | typedef uint16_t u16; 20 | typedef uint8_t u8; 21 | typedef int8_t s8; 22 | typedef int16_t s16; 23 | typedef int32_t s32; 24 | typedef int64_t s64; 25 | 26 | #endif // PROJECT_CTYPES_H 27 | -------------------------------------------------------------------------------- /cyberdog_gazebo/include/ctrl_ros/control_parameters/robot_parameters.hpp: -------------------------------------------------------------------------------- 1 | #ifndef ROBOT_PARAMETERS_HPP_ 2 | #define ROBOT_PARAMETERS_HPP_ 3 | 4 | #include "control_parameters/control_parameters.hpp" 5 | 6 | /** 7 | * @brief ControlParameters shared among all robot controllers 8 | * 9 | */ 10 | class RobotControlParameters : public ControlParameters { 11 | public: 12 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 13 | 14 | /** 15 | * @brief Construct RobotControlParameters 16 | */ 17 | RobotControlParameters() 18 | : ControlParameters( "robot-parameters" ), INIT_PARAMETER( control_mode ), INIT_PARAMETER( gait_id ), INIT_PARAMETER( controller_dt ), INIT_PARAMETER( cheater_mode ), 19 | INIT_PARAMETER( imu_process_noise_position ), INIT_PARAMETER( imu_process_noise_velocity ), INIT_PARAMETER( foot_process_noise_position ), INIT_PARAMETER( foot_sensor_noise_position ), 20 | INIT_PARAMETER( foot_sensor_noise_velocity ), INIT_PARAMETER( foot_height_sensor_noise ), INIT_PARAMETER( use_rc ), INIT_PARAMETER( imu_bias_estimation ), 21 | INIT_PARAMETER( imu_adaptive_gain ), INIT_PARAMETER( gain_acc ), INIT_PARAMETER( speed_offset_trot_10_4 ), INIT_PARAMETER( speed_offset_trot_follow ), 22 | INIT_PARAMETER( speed_offset_trot_medium ), INIT_PARAMETER( speed_offset_trot_slow ), INIT_PARAMETER( speed_offset_trot_fast ), INIT_PARAMETER( speed_offset_trot_24_16 ), 23 | INIT_PARAMETER( speed_offset_trot_8_3 ), INIT_PARAMETER( speed_offset_ballet ), INIT_PARAMETER( speed_offset_bound ), INIT_PARAMETER( speed_offset_pronk ), 24 | INIT_PARAMETER( se_ori_cali_gain ), INIT_PARAMETER( se_ori_cali_offset ), INIT_PARAMETER( filter_type ), INIT_PARAMETER( delt_roll ), INIT_PARAMETER( delt_pitch ), 25 | INIT_PARAMETER( complementaryfilter_source ), INIT_PARAMETER( lcm_debug_switch ) {} 26 | 27 | DECLARE_PARAMETER( s64, control_mode ) 28 | DECLARE_PARAMETER( s64, gait_id ); 29 | DECLARE_PARAMETER( double, controller_dt ) 30 | 31 | // state estimator 32 | DECLARE_PARAMETER( s64, cheater_mode ) 33 | DECLARE_PARAMETER( double, imu_process_noise_position ) 34 | DECLARE_PARAMETER( double, imu_process_noise_velocity ) 35 | DECLARE_PARAMETER( double, foot_process_noise_position ) 36 | DECLARE_PARAMETER( double, foot_sensor_noise_position ) 37 | DECLARE_PARAMETER( double, foot_sensor_noise_velocity ) 38 | DECLARE_PARAMETER( double, foot_height_sensor_noise ) 39 | 40 | DECLARE_PARAMETER( s64, use_rc ) 41 | 42 | DECLARE_PARAMETER( s64, imu_bias_estimation ) 43 | DECLARE_PARAMETER( s64, imu_adaptive_gain ) 44 | DECLARE_PARAMETER( double, gain_acc ) 45 | 46 | DECLARE_PARAMETER( Vec3< double >, speed_offset_trot_10_4 ); 47 | DECLARE_PARAMETER( Vec3< double >, speed_offset_trot_follow ); 48 | DECLARE_PARAMETER( Vec3< double >, speed_offset_trot_medium ); 49 | DECLARE_PARAMETER( Vec3< double >, speed_offset_trot_slow ); 50 | DECLARE_PARAMETER( Vec3< double >, speed_offset_trot_fast ); 51 | DECLARE_PARAMETER( Vec3< double >, speed_offset_trot_24_16 ); 52 | DECLARE_PARAMETER( Vec3< double >, speed_offset_trot_8_3 ); 53 | DECLARE_PARAMETER( Vec3< double >, speed_offset_ballet ); 54 | DECLARE_PARAMETER( Vec3< double >, speed_offset_bound ); 55 | DECLARE_PARAMETER( Vec3< double >, speed_offset_pronk ); 56 | DECLARE_PARAMETER( Vec3< double >, se_ori_cali_gain ); 57 | DECLARE_PARAMETER( Vec3< double >, se_ori_cali_offset ); 58 | 59 | DECLARE_PARAMETER( s64, filter_type ) 60 | 61 | DECLARE_PARAMETER( double, delt_roll ) 62 | DECLARE_PARAMETER( double, delt_pitch ) 63 | DECLARE_PARAMETER( double, complementaryfilter_source ) 64 | DECLARE_PARAMETER( s64, lcm_debug_switch ) 65 | }; 66 | 67 | #endif // ROBOT_PARAMETERS_HPP_ 68 | -------------------------------------------------------------------------------- /cyberdog_gazebo/include/ctrl_ros/cpp_types.hpp: -------------------------------------------------------------------------------- 1 | /*! @file c_types.h 2 | * @brief Common types that are only valid in C++ 3 | * 4 | * This file contains types which are only used in C++ code. This includes 5 | * Eigen types, template types, aliases, ... 6 | */ 7 | 8 | #ifndef PROJECT_CPPTYPES_H 9 | #define PROJECT_CPPTYPES_H 10 | 11 | // TODO: we MUST COMMIT IT 12 | #if defined (__APPLE__) 13 | #undef __ARM_NEON__ 14 | #undef __ARM_NEON 15 | #endif 16 | 17 | 18 | #include 19 | #include "c_types.h" 20 | #include 21 | 22 | // Rotation Matrix 23 | template 24 | using RotMat = typename Eigen::Matrix; 25 | 26 | // 2x1 Vector 27 | template 28 | using Vec2 = typename Eigen::Matrix; 29 | 30 | // 3x1 Vector 31 | template 32 | using Vec3 = typename Eigen::Matrix; 33 | 34 | // 4x1 Vector 35 | template 36 | using Vec4 = typename Eigen::Matrix; 37 | 38 | // 6x1 Vector 39 | template 40 | using Vec6 = Eigen::Matrix; 41 | 42 | // 8x1 Vector 43 | template 44 | using Vec8 = Eigen::Matrix; 45 | 46 | // 9x1 Vector 47 | template 48 | using Vec9 = Eigen::Matrix; 49 | 50 | // 10x1 Vector 51 | template 52 | using Vec10 = Eigen::Matrix; 53 | 54 | // 12x1 Vector 55 | template 56 | using Vec12 = Eigen::Matrix; 57 | 58 | // 18x1 Vector 59 | template 60 | using Vec18 = Eigen::Matrix; 61 | 62 | // 24x1 vector 63 | template 64 | using Vec24 = Eigen::Matrix; 65 | 66 | // 28x1 vector 67 | template 68 | using Vec28 = Eigen::Matrix; 69 | 70 | template 71 | using VecX = Eigen::Matrix; 72 | 73 | // 3x3 Matrix 74 | template 75 | using Mat3 = typename Eigen::Matrix; 76 | 77 | // 4x1 Vector 78 | template 79 | using Quat = typename Eigen::Matrix; 80 | 81 | // Spatial Vector (6x1, all subspaces) 82 | template 83 | using SVec = typename Eigen::Matrix; 84 | 85 | // Spatial Transform (6x6) 86 | template 87 | using SXform = typename Eigen::Matrix; 88 | 89 | // 6x6 Matrix 90 | template 91 | using Mat6 = typename Eigen::Matrix; 92 | 93 | // 12x12 Matrix 94 | template 95 | using Mat12 = typename Eigen::Matrix; 96 | 97 | // 18x18 Matrix 98 | template 99 | using Mat18 = Eigen::Matrix; 100 | 101 | // 24x12 Matrix 102 | template 103 | using Mat2412 = Eigen::Matrix; 104 | 105 | // 28x28 Matrix 106 | template 107 | using Mat28 = Eigen::Matrix; 108 | 109 | // 3x4 Matrix 110 | template 111 | using Mat34 = Eigen::Matrix; 112 | 113 | // 3x4 Matrix 114 | template 115 | using Mat23 = Eigen::Matrix; 116 | 117 | // 4x4 Matrix 118 | template 119 | using Mat4 = typename Eigen::Matrix; 120 | 121 | // 10x1 Vector 122 | template 123 | using MassProperties = typename Eigen::Matrix; 124 | 125 | // Dynamically sized vector 126 | template 127 | using DVec = typename Eigen::Matrix; 128 | 129 | // Dynamically sized matrix 130 | template 131 | using DMat = typename Eigen::Matrix; 132 | 133 | // Dynamically sized matrix with spatial vector columns 134 | template 135 | using D6Mat = typename Eigen::Matrix; 136 | 137 | // Dynamically sized matrix with cartesian vector columns 138 | template 139 | using D3Mat = typename Eigen::Matrix; 140 | 141 | // std::vector (a list) of Eigen things 142 | template 143 | using vectorAligned = typename std::vector>; 144 | 145 | enum class RobotType { CYBERDOG, MINI_CYBERDOG }; 146 | 147 | #endif // PROJECT_CPPTYPES_H 148 | -------------------------------------------------------------------------------- /cyberdog_gazebo/include/ctrl_ros/parameters.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by hujian on 2021/5/28. 3 | // 4 | 5 | #ifndef SIM_PARAMETERS_H 6 | #define SIM_PARAMETERS_H 7 | 8 | #define IMU_CALIBRATE_FILE_PATH "/mnt/misc/imu_calibrate_param.yaml" 9 | 10 | #define OFFSET_CALIBRATE_PARAM_FILE_PATH "/mnt/misc/offset_calibrate_param.yaml" 11 | 12 | #define POSE_CALIBRATE_PARAM_FILE_PATH "/mnt/misc/pose_calibrate_param.yaml" 13 | 14 | #define SPEED_CALIBRATE_FILE_PATH "/mnt/misc/speed_calibrate_param.yaml" 15 | 16 | #define SPEED_PARAMETER_CALIBRATE_CONTROL_DEBUG 1 17 | 18 | #define SPI_YAML_SH_PATH "/robot-software/config/mini-cheetah-spi.yaml" 19 | #define SPI_YAML_HAND_SH_PATH "/robot-software/config/mini-cheetah-spi-hand.yaml" 20 | #define SPI_YAML_HAND_SH_BAK_PATH "/robot-software/config/mini-cheetah-spi-hand.yaml.bak" 21 | #define MANAGER_SCRIPT_PATH "/mnt/UDISK/test/manager.sh" 22 | 23 | #define ROBOT_INTERFACE_UPDATE_PERIOD (1.f / 60.f) 24 | #define INTERFACE_LCM_NAME "interface" 25 | #define TIMES_TO_RESEND_CONTROL_PARAM 5 26 | 27 | #define K_WORDS_PER_MESSAGE 66 28 | #define ZERO_FLAG 0xdead 29 | 30 | #define BUDDA_Q_SCALE 6.f 31 | 32 | #define MAX_STACK_SIZE 16384 // 16KB of stack 33 | #define TASK_PRIORITY 49 // linux priority, this is not the nice value 34 | 35 | #define DEFAULT_TERRAIN_FILE "/default-terrain.yaml" 36 | #define DEFAULT_USER_FILE "/default-user-parameters-file.yaml" 37 | 38 | #define SIM_LCM_NAME "simulator_state" 39 | 40 | // Normal robot states 41 | #define K_PASSIVE 0 42 | #define K_BALANCE_STAND 3 43 | #define K_LOCOMOTION 4 44 | #define K_LOCOMOTION_TEST 5 45 | #define K_RECOVERY_STAND 6 46 | #define K_VISION 8 47 | #define K_BACKFLIP 9 48 | #define K_FRONTJUMP 11 49 | #define K_FRONTFLIP 12 50 | 51 | // Specific control states 52 | #define K_JOINT_PD 51 53 | #define K_IMPEDANCE_CONTROL 52 54 | #define K_MOTION 62 55 | 56 | //Extra control states 57 | #define K_PUREDAMPER_J 71 58 | #define K_INVALID 100 59 | 60 | // new gait 61 | #define K_Dance 101 62 | #define K_TwoLegStand_PD 302 63 | 64 | #define IMU_SERIAL_COM "/dev/ttyUSB0" 65 | #define BMS_SERIAL_COM "/dev/ttyS3" 66 | 67 | #define YAML_COLLECTION_NAME_KEY "__collection-name__" 68 | 69 | //all print control DEBUG 70 | #define PRINT_CONVEX_MPC_DEBUG 0 71 | 72 | #define GRAVITY 9.81 73 | 74 | #endif //SIM_PARAMETERS_H 75 | -------------------------------------------------------------------------------- /cyberdog_gazebo/include/ctrl_ros/sim_utilities/gamepad_command.hpp: -------------------------------------------------------------------------------- 1 | /*! @file gamepad_command.hpp 2 | * @brief The GamepadCommand type containing joystick information 3 | */ 4 | 5 | #ifndef PROJECT_GAMEPADCOMMAND_H 6 | #define PROJECT_GAMEPADCOMMAND_H 7 | 8 | #include "utilities/utilities.h" 9 | #include "cpp_types.hpp" 10 | #include "gamepad_lcmt.hpp" 11 | 12 | //using namespace dreame_dog; 13 | 14 | /*! 15 | * The state of the gamepad 16 | */ 17 | struct GamepadCommand { 18 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 19 | 20 | /*! 21 | * Construct a gamepad and set to zero. 22 | */ 23 | GamepadCommand() { zero(); } 24 | 25 | bool leftBumper, rightBumper, leftTriggerButton, rightTriggerButton, back, 26 | start, a, b, x, y, leftStickButton, rightStickButton, logitechButton; 27 | 28 | Vec2 leftStickAnalog, rightStickAnalog; 29 | float leftTriggerAnalog, rightTriggerAnalog; 30 | 31 | /*! 32 | * Set all values to zero 33 | */ 34 | void zero() { 35 | leftBumper = false; 36 | rightBumper = false; 37 | leftTriggerButton = false; 38 | rightTriggerButton = false; 39 | back = false; 40 | start = false; 41 | a = false; 42 | b = false; 43 | x = false; 44 | y = false; 45 | leftStickButton = false; 46 | rightStickButton = false; 47 | 48 | leftTriggerAnalog = 0; 49 | rightTriggerAnalog = 0; 50 | leftStickAnalog = Vec2::Zero(); 51 | rightStickAnalog = Vec2::Zero(); 52 | } 53 | 54 | /*! 55 | * Set the values from an LCM message 56 | * @param lcmt : LCM message 57 | */ 58 | void set(const gamepad_lcmt* lcmt) { 59 | leftBumper = lcmt->leftBumper; 60 | rightBumper = lcmt->rightBumper; 61 | leftTriggerButton = lcmt->leftTriggerButton; 62 | rightTriggerButton = lcmt->rightTriggerButton; 63 | back = lcmt->back; 64 | start = lcmt->start; 65 | a = lcmt->a; 66 | x = lcmt->x; 67 | b = lcmt->b; 68 | y = lcmt->y; 69 | leftStickButton = lcmt->leftStickButton; 70 | rightStickButton = lcmt->rightStickButton; 71 | leftTriggerAnalog = lcmt->leftTriggerAnalog; 72 | rightTriggerAnalog = lcmt->rightTriggerAnalog; 73 | for (int i = 0; i < 2; i++) { 74 | leftStickAnalog[i] = lcmt->leftStickAnalog[i]; 75 | rightStickAnalog[i] = lcmt->rightStickAnalog[i]; 76 | } 77 | } 78 | 79 | /*! 80 | * Copy the values into an LCM message 81 | * @param lcmt : LCM message 82 | */ 83 | void get(gamepad_lcmt* lcmt) { 84 | lcmt->leftBumper = leftBumper; 85 | lcmt->rightBumper = rightBumper; 86 | lcmt->leftTriggerButton = leftTriggerButton; 87 | lcmt->rightTriggerButton = rightTriggerButton; 88 | lcmt->back = back; 89 | lcmt->start = start; 90 | lcmt->a = a; 91 | lcmt->x = x; 92 | lcmt->b = b; 93 | lcmt->y = y; 94 | lcmt->leftStickButton = leftStickButton; 95 | lcmt->rightStickButton = rightStickButton; 96 | lcmt->leftTriggerAnalog = leftTriggerAnalog; 97 | lcmt->rightTriggerAnalog = rightTriggerAnalog; 98 | for (int i = 0; i < 2; i++) { 99 | lcmt->leftStickAnalog[i] = leftStickAnalog[i]; 100 | lcmt->rightStickAnalog[i] = rightStickAnalog[i]; 101 | } 102 | } 103 | 104 | /*! 105 | * The Logitech F310's seem to do a bad job of returning to zero exactly, so a 106 | * deadband around zero is useful when integrating joystick commands 107 | * @param f : The deadband 108 | */ 109 | void GamepadApplyDeadband( float f ) { 110 | EigenApplyDeadband( leftStickAnalog, f ); 111 | EigenApplyDeadband( rightStickAnalog, f ); 112 | leftTriggerAnalog = ApplyDeadband( leftTriggerAnalog, f ); 113 | rightTriggerAnalog = ApplyDeadband( rightTriggerAnalog, f ); 114 | } 115 | 116 | /*! 117 | * Represent as human-readable string. 118 | * @return string representing state 119 | */ 120 | std::string ToString() { 121 | std::string result = "Result:\nleftBumper: " + BoolToString( leftBumper ) + "\n" + "rightBumper: " + BoolToString( rightBumper ) + "\n" 122 | + "leftTriggerButton: " + BoolToString( leftTriggerButton ) + "\n" + "rightTriggerButton: " + BoolToString( rightTriggerButton ) + "\n" + "back: " + BoolToString( back ) 123 | + "\n" + "start: " + BoolToString( start ) + "\n" + "a: " + BoolToString( a ) + "\n" + "b: " + BoolToString( b ) + "\n" + "x: " + BoolToString( x ) + "\n" 124 | + "y: " + BoolToString( y ) + "\n" + "leftStickButton: " + BoolToString( leftStickButton ) + "\n" + "rightStickButton: " + BoolToString( rightStickButton ) + "\n" 125 | + "leftTriggerAnalog: " + std::to_string( leftTriggerAnalog ) + "\n" + "rightTriggerAnalog: " + std::to_string( rightTriggerAnalog ) + "\n" 126 | + "leftStickAnalog: " + EigenToString( leftStickAnalog ) + "\n" + "rightStickAnalog: " + EigenToString( rightStickAnalog ) + "\n"; 127 | return result; 128 | } 129 | }; 130 | 131 | #endif // PROJECT_DRIVERCOMMAND_H 132 | -------------------------------------------------------------------------------- /cyberdog_gazebo/include/ctrl_ros/sim_utilities/imu_types.hpp: -------------------------------------------------------------------------------- 1 | /*! @file imu_types.hpp 2 | * @brief Data from IMUs 3 | */ 4 | 5 | #ifndef PROJECT_IMUTYPES_H 6 | #define PROJECT_IMUTYPES_H 7 | 8 | #include "cpp_types.hpp" 9 | 10 | /*! 11 | * Mini Cheetah's IMU 12 | */ 13 | struct VectorNavData { 14 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 15 | 16 | VectorNavData() { 17 | accelerometer = Vec3(0, 0, 9.81); 18 | gyro.setZero(); 19 | quat = Quat(0, 0, 0, 1); 20 | } 21 | 22 | Vec3 accelerometer; 23 | Vec3 gyro; 24 | Quat quat; 25 | // todo is there status for the vectornav? 26 | }; 27 | 28 | /*! 29 | * "Cheater" state sent to the robot from simulator 30 | */ 31 | template 32 | struct CheaterState { 33 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 34 | 35 | Quat orientation; 36 | Vec3 position; 37 | Vec3 omegaBody; 38 | Vec3 vBody; 39 | Vec3 acceleration; 40 | }; 41 | 42 | #endif // PROJECT_IMUTYPES_H 43 | -------------------------------------------------------------------------------- /cyberdog_gazebo/include/ctrl_ros/sim_utilities/simulator_message.hpp: -------------------------------------------------------------------------------- 1 | /*! @file simulator_message.hpp 2 | * @brief Messages sent to/from the development simulator 3 | * 4 | * These messsages contain all data that is exchanged between the robot program 5 | * and the simulator using shared memory. This is basically everything except 6 | * for debugging logs, which are handled by LCM instead 7 | */ 8 | 9 | #ifndef PROJECT_SIMULATORTOROBOTMESSAGE_H 10 | #define PROJECT_SIMULATORTOROBOTMESSAGE_H 11 | 12 | #include "control_parameters/control_parameter_interface.hpp" 13 | #include "sim_utilities/gamepad_command.hpp" 14 | #include "sim_utilities/imu_types.hpp" 15 | #include "sim_utilities/spine_board.hpp" 16 | #include "sim_utilities/visualization_data.hpp" 17 | #include "utilities/shared_memory.hpp" 18 | 19 | /*! 20 | * The mode for the simulator 21 | */ 22 | enum class SimulatorMode { 23 | RUN_CONTROL_PARAMETERS, // don't run the robot controller, just process 24 | // Control Parameters 25 | RUN_CONTROLLER, // run the robot controller 26 | DO_NOTHING, // just to check connection 27 | EXIT // quit! 28 | }; 29 | 30 | /*! 31 | * A plain message from the simulator to the robot 32 | */ 33 | struct SimulatorToRobotMessage { 34 | int value0[2];//value*: tmp for struct 8bytes alignment 35 | 36 | int value1; 37 | 38 | int value2; 39 | GamepadCommand gamepadCommand;// joystick 40 | RobotType robotType;// which robot the simulator thinks we are simulating 41 | 42 | 43 | // imu data 44 | VectorNavData vectorNav; 45 | CheaterState cheaterState; 46 | 47 | // leg data 48 | SpiData spiData; 49 | // TODO: remove tiboard related later 50 | // TiBoardData tiBoardData[4]; 51 | ControlParameterRequest controlParameterRequest; 52 | 53 | SimulatorMode mode; 54 | //int value2; 55 | }; 56 | 57 | /*! 58 | * A plain message from the robot to the simulator 59 | */ 60 | struct RobotToSimulatorMessage { 61 | 62 | RobotType robotType; 63 | SpiCommand spiCommand; 64 | // TiBoardCommand tiBoardCommand[4]; 65 | 66 | VisualizationData visualizationData; 67 | CheetahVisualization mainCheetahVisualization; 68 | ControlParameterResponse controlParameterResponse; 69 | 70 | char errorMessage[2056]; 71 | }; 72 | 73 | /*! 74 | * All the data shared between the robot and the simulator 75 | */ 76 | struct SimulatorMessage { 77 | RobotToSimulatorMessage robotToSim; 78 | SimulatorToRobotMessage simToRobot; 79 | }; 80 | 81 | #endif // PROJECT_SIMULATORTOROBOTMESSAGE_H 82 | -------------------------------------------------------------------------------- /cyberdog_gazebo/include/ctrl_ros/sim_utilities/spine_board.hpp: -------------------------------------------------------------------------------- 1 | /*! @file spine_board.hpp 2 | * @brief Spine Board Code, used to simulate the SpineBoard. 3 | * 4 | * This is mostly a copy of the exact code that runs on the SpineBoard 5 | */ 6 | 7 | #ifndef PROJECT_SPINEBOARD_H 8 | #define PROJECT_SPINEBOARD_H 9 | 10 | #include "c_types.h" 11 | #include 12 | 13 | /*! 14 | * Command to spine board 15 | */ 16 | struct SpiCommand { 17 | float q_des_abad[4]; 18 | float q_des_hip[4]; 19 | float q_des_knee[4]; 20 | 21 | float qd_des_abad[4]; 22 | float qd_des_hip[4]; 23 | float qd_des_knee[4]; 24 | 25 | float kp_abad[4]; 26 | float kp_hip[4]; 27 | float kp_knee[4]; 28 | 29 | float kd_abad[4]; 30 | float kd_hip[4]; 31 | float kd_knee[4]; 32 | 33 | float tau_abad_ff[4]; 34 | float tau_hip_ff[4]; 35 | float tau_knee_ff[4]; 36 | 37 | int32_t flags[4]; 38 | 39 | explicit SpiCommand() { 40 | memset(this, 0, sizeof(SpiCommand)); 41 | } 42 | }; 43 | 44 | /*! 45 | * Data from spine board 46 | */ 47 | struct SpiData { 48 | explicit SpiData() { memset(this, 0, sizeof(SpiData)); } 49 | 50 | float q_abad[4]; 51 | float q_hip[4]; 52 | float q_knee[4]; 53 | float qd_abad[4]; 54 | float qd_hip[4]; 55 | float qd_knee[4]; 56 | int32_t flags[12]; 57 | int32_t spi_driver_status; 58 | 59 | float tau_abad[4]; 60 | float tau_hip[4]; 61 | float tau_knee[4]; 62 | 63 | int16_t tmp_abad[ 4 ]; 64 | int16_t tmp_hip[ 4 ]; 65 | int16_t tmp_knee[ 4 ]; 66 | 67 | uint8_t reserve[ 40 ]; 68 | }; 69 | 70 | /*! 71 | * Spine board control logic 72 | */ 73 | class SpineBoard { 74 | public: 75 | SpineBoard() {} 76 | void init(float side_sign, s32 board); 77 | void run(); 78 | void resetData(); 79 | void resetCommand(); 80 | SpiCommand* cmd = nullptr; 81 | SpiData* data = nullptr; 82 | float torque_out[3]; 83 | 84 | private: 85 | float side_sign; 86 | s32 board_num; 87 | const float max_torque[3] = {17.f, 24.f, 26.f}; // TODO CHECK WITH BEN 88 | const float wimp_torque[3] = {6.f, 6.f, 6.f}; // TODO CHECK WITH BEN 89 | const float disabled_torque[3] = {0.f, 0.f, 0.f}; 90 | const float q_limit_p[3] = {1.5f, 5.0f, 0.f}; 91 | const float q_limit_n[3] = {-1.5f, -5.0f, 0.f}; 92 | const float kp_softstop = 100.f; 93 | const float kd_softstop = 0.4f; 94 | s32 iter_counter = 0; 95 | }; 96 | 97 | #endif // PROJECT_SPINEBOARD_H 98 | -------------------------------------------------------------------------------- /cyberdog_gazebo/include/ctrl_ros/sim_utilities/visualization_data.hpp: -------------------------------------------------------------------------------- 1 | /*! @file VisualizationData.h 2 | * @brief Data sent from robot code to simulator GUI for debugging 3 | * 4 | */ 5 | 6 | #ifndef VISUALIZATION_DATA_H 7 | #define VISUALIZATION_DATA_H 8 | 9 | #define VISUALIZATION_MAX_PATH_POINTS 2000 10 | #define VISUALIZATION_MAX_PATHS 10 11 | #define VISUALIZATION_MAX_ITEMS 10000 12 | #define VISUALIZATION_MAX_CHEETAHS 0 13 | 14 | #define VISUALIZATION_MAX_MESHES 5 15 | #define VISUALIZATION_MAX_MESH_GRID 150 16 | 17 | #include "cpp_types.hpp" 18 | 19 | /*! 20 | * Debugging sphere 21 | */ 22 | struct SphereVisualization { 23 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 24 | 25 | Vec3 position; 26 | Vec4 color; 27 | double radius; 28 | }; 29 | 30 | /*! 31 | * Debugging box 32 | */ 33 | struct BlockVisualization { 34 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 35 | 36 | Vec3 dimension; 37 | Vec3 corner_position; 38 | Vec3 rpy; 39 | Vec4 color; 40 | }; 41 | 42 | /*! 43 | * Debugging arrow 44 | */ 45 | struct ArrowVisualization { 46 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 47 | 48 | Vec3 base_position; 49 | Vec3 direction; 50 | Vec4 color; 51 | float head_width; 52 | float head_length; 53 | float shaft_width; 54 | }; 55 | 56 | /*! 57 | * Debugging robot (draws the same type of robot as currently simulating) 58 | */ 59 | struct CheetahVisualization { 60 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 61 | 62 | Vec12 q; 63 | Quat quat; 64 | Vec3 p; 65 | Vec4 color; 66 | }; 67 | 68 | /*! 69 | * Debugging "path" 70 | */ 71 | struct PathVisualization { 72 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 73 | 74 | size_t num_points = 0; 75 | Vec4 color; 76 | Vec3 position[VISUALIZATION_MAX_PATH_POINTS]; 77 | void clear() { 78 | num_points = 0; 79 | } 80 | }; 81 | 82 | /*! 83 | * Debugging Cone 84 | */ 85 | struct ConeVisualization { 86 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 87 | 88 | Vec3 point_position; 89 | Vec3 direction; 90 | Vec4 color; 91 | double radius; 92 | }; 93 | 94 | /*! 95 | * Mesh Visualization 96 | */ 97 | struct MeshVisualization { 98 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 99 | 100 | Vec3 left_corner; 101 | Eigen::Matrix height_map; 102 | 103 | int rows, cols; 104 | 105 | float grid_size; 106 | float height_max; 107 | float height_min; 108 | }; 109 | 110 | 111 | /*! 112 | * Collection of all debugging data 113 | */ 114 | struct VisualizationData { 115 | size_t num_paths = 0, num_arrows = 0, num_cones = 0, num_spheres = 0, 116 | num_blocks = 0, num_meshes = 0; 117 | SphereVisualization spheres[VISUALIZATION_MAX_ITEMS]; 118 | BlockVisualization blocks[VISUALIZATION_MAX_ITEMS]; 119 | ArrowVisualization arrows[VISUALIZATION_MAX_ITEMS]; 120 | ConeVisualization cones[VISUALIZATION_MAX_ITEMS]; 121 | PathVisualization paths[VISUALIZATION_MAX_PATHS]; 122 | MeshVisualization meshes[VISUALIZATION_MAX_MESHES]; 123 | 124 | /*! 125 | * Remove all debug data 126 | */ 127 | void clear() { 128 | num_paths = 0, num_arrows = 0, num_cones = 0, num_spheres = 0, num_blocks = 0; 129 | num_meshes = 0; 130 | } 131 | 132 | /*! 133 | * Add a new sphere 134 | * @return A sphere, or nullptr if there isn't enough room 135 | */ 136 | SphereVisualization* addSphere() { 137 | if(num_spheres < VISUALIZATION_MAX_ITEMS) { 138 | return &spheres[num_spheres++]; 139 | } 140 | return nullptr; 141 | } 142 | 143 | /*! 144 | * Add a new box 145 | * @return A box, or nullptr if there isn't enough room 146 | */ 147 | BlockVisualization* addBlock() { 148 | if(num_blocks < VISUALIZATION_MAX_ITEMS) { 149 | return &blocks[num_blocks++]; 150 | } 151 | return nullptr; 152 | } 153 | 154 | /*! 155 | * Add a new arrow 156 | * @return An arrow, or nullptr if there isn't enough room 157 | */ 158 | ArrowVisualization* addArrow() { 159 | if(num_arrows < VISUALIZATION_MAX_ITEMS) { 160 | return &arrows[num_arrows++]; 161 | } 162 | return nullptr; 163 | } 164 | 165 | /*! 166 | * Add a new cone 167 | * @return A cone, or nullptr if there isn't enough room 168 | */ 169 | ConeVisualization* addCone() { 170 | if(num_cones < VISUALIZATION_MAX_ITEMS) { 171 | return &cones[num_cones++]; 172 | } 173 | return nullptr; 174 | } 175 | 176 | /*! 177 | * Add a new path 178 | * @return A path, or nullptr if there isn't enough room 179 | */ 180 | PathVisualization* addPath() { 181 | if(num_paths < VISUALIZATION_MAX_PATHS) { 182 | auto* path = &paths[num_paths++]; 183 | path->clear(); 184 | return path; 185 | } 186 | return nullptr; 187 | } 188 | 189 | /*! 190 | * Add a new Mesh 191 | * @return A mesh, or nullptr if there isn't enough room 192 | */ 193 | MeshVisualization* addMesh() { 194 | if(num_paths < VISUALIZATION_MAX_MESHES) { 195 | return &meshes[num_meshes++]; 196 | } 197 | return nullptr; 198 | } 199 | }; 200 | 201 | #endif 202 | -------------------------------------------------------------------------------- /cyberdog_gazebo/include/foot_contact_plugin.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023-2023 Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | #ifndef FOOT_CONTACT_PLUGIN_HPP 15 | #define FOOT_CONTACT_PLUGIN_HPP 16 | 17 | #include 18 | 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | #include "gazebo_foot_contact.hpp" 27 | 28 | 29 | namespace gazebo 30 | { 31 | class FootContactPlugin : public SensorPlugin 32 | { 33 | 34 | public: 35 | /** 36 | * @brief Construct a new Foot Contact Plugin object 37 | * 38 | */ 39 | FootContactPlugin(); 40 | 41 | /** 42 | * @brief Destroy the Foot Contact Plugin object 43 | * 44 | */ 45 | virtual ~FootContactPlugin(); 46 | 47 | /** 48 | * @brief Load the sensor plugin. 49 | * 50 | * @param _sensor Pointer to the sensor that loaded this plugin. 51 | * @param _sdf SDF element that describes the plugin. 52 | */ 53 | virtual void Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sdf); 54 | 55 | private: 56 | /** 57 | * @brief Callback that receives the contact sensor's update signal. 58 | * 59 | */ 60 | virtual void OnUpdate(); 61 | 62 | // Pointer to the contact sensor 63 | sensors::ContactSensorPtr parentSensor; 64 | 65 | // Connection that maintains a link between the contact sensor's 66 | // updated signal and the OnUpdate callback. 67 | event::ConnectionPtr updateConnection; 68 | }; 69 | } 70 | #endif 71 | -------------------------------------------------------------------------------- /cyberdog_gazebo/include/gazebo_foot_contact.hpp: -------------------------------------------------------------------------------- 1 | /** THIS IS AN AUTOMATICALLY GENERATED FILE. DO NOT MODIFY 2 | * BY HAND!! 3 | * 4 | * Generated by lcm-gen 5 | **/ 6 | 7 | #ifndef __gazebo_foot_contact_hpp__ 8 | #define __gazebo_foot_contact_hpp__ 9 | 10 | #include 11 | 12 | 13 | 14 | class gazebo_foot_contact 15 | { 16 | public: 17 | double contact_force[12]; 18 | 19 | double euler_angle[12]; 20 | 21 | double contact_force_W[12]; 22 | 23 | public: 24 | /** 25 | * Encode a message into binary form. 26 | * 27 | * @param buf The output buffer. 28 | * @param offset Encoding starts at thie byte offset into @p buf. 29 | * @param maxlen Maximum number of bytes to write. This should generally be 30 | * equal to getEncodedSize(). 31 | * @return The number of bytes encoded, or <0 on error. 32 | */ 33 | inline int encode(void *buf, int offset, int maxlen) const; 34 | 35 | /** 36 | * Check how many bytes are required to encode this message. 37 | */ 38 | inline int getEncodedSize() const; 39 | 40 | /** 41 | * Decode a message from binary form into this instance. 42 | * 43 | * @param buf The buffer containing the encoded message. 44 | * @param offset The byte offset into @p buf where the encoded message starts. 45 | * @param maxlen The maximum number of bytes to read while decoding. 46 | * @return The number of bytes decoded, or <0 if an error occured. 47 | */ 48 | inline int decode(const void *buf, int offset, int maxlen); 49 | 50 | /** 51 | * Retrieve the 64-bit fingerprint identifying the structure of the message. 52 | * Note that the fingerprint is the same for all instances of the same 53 | * message type, and is a fingerprint on the message type definition, not on 54 | * the message contents. 55 | */ 56 | inline static int64_t getHash(); 57 | 58 | /** 59 | * Returns "gazebo_foot_contact" 60 | */ 61 | inline static const char* getTypeName(); 62 | 63 | // LCM support functions. Users should not call these 64 | inline int _encodeNoHash(void *buf, int offset, int maxlen) const; 65 | inline int _getEncodedSizeNoHash() const; 66 | inline int _decodeNoHash(const void *buf, int offset, int maxlen); 67 | inline static uint64_t _computeHash(const __lcm_hash_ptr *p); 68 | }; 69 | 70 | int gazebo_foot_contact::encode(void *buf, int offset, int maxlen) const 71 | { 72 | int pos = 0, tlen; 73 | int64_t hash = getHash(); 74 | 75 | tlen = __int64_t_encode_array(buf, offset + pos, maxlen - pos, &hash, 1); 76 | if(tlen < 0) return tlen; else pos += tlen; 77 | 78 | tlen = this->_encodeNoHash(buf, offset + pos, maxlen - pos); 79 | if (tlen < 0) return tlen; else pos += tlen; 80 | 81 | return pos; 82 | } 83 | 84 | int gazebo_foot_contact::decode(const void *buf, int offset, int maxlen) 85 | { 86 | int pos = 0, thislen; 87 | 88 | int64_t msg_hash; 89 | thislen = __int64_t_decode_array(buf, offset + pos, maxlen - pos, &msg_hash, 1); 90 | if (thislen < 0) return thislen; else pos += thislen; 91 | if (msg_hash != getHash()) return -1; 92 | 93 | thislen = this->_decodeNoHash(buf, offset + pos, maxlen - pos); 94 | if (thislen < 0) return thislen; else pos += thislen; 95 | 96 | return pos; 97 | } 98 | 99 | int gazebo_foot_contact::getEncodedSize() const 100 | { 101 | return 8 + _getEncodedSizeNoHash(); 102 | } 103 | 104 | int64_t gazebo_foot_contact::getHash() 105 | { 106 | static int64_t hash = static_cast(_computeHash(NULL)); 107 | return hash; 108 | } 109 | 110 | const char* gazebo_foot_contact::getTypeName() 111 | { 112 | return "gazebo_foot_contact"; 113 | } 114 | 115 | int gazebo_foot_contact::_encodeNoHash(void *buf, int offset, int maxlen) const 116 | { 117 | int pos = 0, tlen; 118 | 119 | tlen = __double_encode_array(buf, offset + pos, maxlen - pos, &this->contact_force[0], 12); 120 | if(tlen < 0) return tlen; else pos += tlen; 121 | 122 | tlen = __double_encode_array(buf, offset + pos, maxlen - pos, &this->euler_angle[0], 12); 123 | if(tlen < 0) return tlen; else pos += tlen; 124 | 125 | tlen = __double_encode_array(buf, offset + pos, maxlen - pos, &this->contact_force_W[0], 12); 126 | if(tlen < 0) return tlen; else pos += tlen; 127 | 128 | return pos; 129 | } 130 | 131 | int gazebo_foot_contact::_decodeNoHash(const void *buf, int offset, int maxlen) 132 | { 133 | int pos = 0, tlen; 134 | 135 | tlen = __double_decode_array(buf, offset + pos, maxlen - pos, &this->contact_force[0], 12); 136 | if(tlen < 0) return tlen; else pos += tlen; 137 | 138 | tlen = __double_decode_array(buf, offset + pos, maxlen - pos, &this->euler_angle[0], 12); 139 | if(tlen < 0) return tlen; else pos += tlen; 140 | 141 | tlen = __double_decode_array(buf, offset + pos, maxlen - pos, &this->contact_force_W[0], 12); 142 | if(tlen < 0) return tlen; else pos += tlen; 143 | 144 | return pos; 145 | } 146 | 147 | int gazebo_foot_contact::_getEncodedSizeNoHash() const 148 | { 149 | int enc_size = 0; 150 | enc_size += __double_encoded_array_size(NULL, 12); 151 | enc_size += __double_encoded_array_size(NULL, 12); 152 | enc_size += __double_encoded_array_size(NULL, 12); 153 | return enc_size; 154 | } 155 | 156 | uint64_t gazebo_foot_contact::_computeHash(const __lcm_hash_ptr *) 157 | { 158 | uint64_t hash = 0xdb59eefda79a1cffLL; 159 | return (hash<<1) + ((hash>>63)&1); 160 | } 161 | 162 | #endif 163 | -------------------------------------------------------------------------------- /cyberdog_gazebo/include/lcmhandler.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023-2023 Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | #ifndef LCM_HANDLER_HPP__ 15 | #define LCM_HANDLER_HPP__ 16 | 17 | #include 18 | 19 | #include "simulator_lcmt.hpp" 20 | #include "gamepad_lcmt.hpp" 21 | #include "sim_utilities/gamepad_command.hpp" 22 | 23 | 24 | namespace gazebo 25 | { 26 | class LCMHandler 27 | { 28 | public: 29 | 30 | /** 31 | * @brief Construct a new LCMHandler object 32 | * 33 | */ 34 | LCMHandler(); 35 | 36 | /** 37 | * @brief Receive gamepad command messages 38 | * 39 | * @return GamepadCommand 40 | */ 41 | GamepadCommand ReceiveGPC(); 42 | 43 | /** 44 | * @brief Send simlator state message by lcm 45 | * 46 | * @param _lcm_sim_handler simlator state message 47 | */ 48 | void SendSimData(simulator_lcmt &_lcm_sim_handler); 49 | 50 | /** 51 | * @brief Return true if lcmhandler receive a lcm message 52 | * 53 | * @return true Lcmhandler receive a lcm message 54 | * @return false No lcm message is received 55 | */ 56 | bool HasEvent(); 57 | 58 | private: 59 | 60 | /** 61 | * @brief Handle gamepad command messages 62 | * 63 | * @param rbuf 64 | * @param chan 65 | * @param msg 66 | */ 67 | void HandleCommand(const lcm::ReceiveBuffer *rbuf, const std::string &chan, const gamepad_lcmt *msg); 68 | 69 | lcm::LCM lcm_; 70 | gamepad_lcmt lcm_gamepad_; 71 | 72 | GamepadCommand gamepad_command_; 73 | }; 74 | 75 | } 76 | 77 | #endif //LCM_HANDLER_HPP__ -------------------------------------------------------------------------------- /cyberdog_gazebo/include/legged_simparam.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023-2023 Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | #ifndef _LEGGED_SIMPARAM_HPP__ 15 | #define _LEGGED_SIMPARAM_HPP__ 16 | 17 | #include 18 | 19 | #include "rclcpp/rclcpp.hpp" 20 | #include 21 | 22 | #include "ctrl_ros/cpp_types.hpp" 23 | #include "utilities/shared_memory.hpp" 24 | #include "sim_utilities/simulator_message.hpp" 25 | 26 | #include "ctrl_ros/control_parameters/control_parameters.hpp" 27 | #include "ctrl_ros/control_parameters/robot_parameters.hpp" 28 | #include "node_executor.hpp" 29 | 30 | namespace gazebo 31 | { 32 | struct ParamHandler 33 | { 34 | std::string name; 35 | ControlParameterValueKind kind; 36 | ControlParameterValue value; 37 | }; 38 | 39 | class SimParam 40 | { 41 | public: 42 | /** 43 | * @brief Construct a new Sim Param object 44 | * 45 | * @param model_name name of robot 46 | * @param node_executor node executor to subscribe yaml message 47 | */ 48 | SimParam(std::string model_name, NodeExc* node_executor); 49 | 50 | /** 51 | * @brief Build connection to control program at the first run 52 | * 53 | */ 54 | void FirstRun(); 55 | 56 | /** 57 | * @brief Send sharedmemory data to control program 58 | * 59 | * @param _SimToRobot sharedmemory data to control program 60 | */ 61 | void SendSMData(SimulatorToRobotMessage _SimToRobot); 62 | 63 | /** 64 | * @brief Receive sharedmemory data from control program 65 | * 66 | * @return SpiCommand sharedmemory data from control program 67 | */ 68 | SpiCommand ReceiveSMData(); 69 | 70 | /** 71 | * @brief Receive YamlParam topic message 72 | * 73 | */ 74 | void ReceiveTopic(); 75 | 76 | /** 77 | * @brief Set the LcmHasEvent if gamepad lcm message is received 78 | * 79 | * @return true Gamepad lcm message is received 80 | * @return false No gamepad lcm message is received 81 | */ 82 | void LcmHasEvent(){lcm_has_event_ = true;}; 83 | 84 | private: 85 | 86 | /** 87 | * @brief Load simulator and control parameter from yaml files 88 | * 89 | */ 90 | void LoadYaml(); 91 | 92 | /** 93 | * @brief Send ControlParameter by shared memory to control program 94 | * 95 | * @param name Name of ControlParameter 96 | * @param value Value of ControlParameter 97 | * @param kind Kind of ControlParameter 98 | * @param isUser true for userparameter 99 | * false for robotparameter 100 | */ 101 | void SendControlParameter( const std::string& name, ControlParameterValue value, ControlParameterValueKind kind, bool isUser ); 102 | 103 | /** 104 | * @brief Handle error message if control program has a error 105 | * 106 | */ 107 | void HandleControlError(); 108 | 109 | /** 110 | * @brief Handle YamlParam topic message 111 | * 112 | * @param msg YamlParam topic message 113 | */ 114 | void HandleYamlParam(const cyberdog_msg::msg::YamlParam::SharedPtr msg); 115 | 116 | 117 | SharedMemoryObject shared_memory_; 118 | 119 | RobotType robotType; 120 | ControlParameters user_parameters_; 121 | RobotControlParameters robot_parameters_; 122 | bool lcm_has_event_; 123 | 124 | // ros2 node to receive YamlParam topic 125 | std::shared_ptr gazebo_node_; 126 | rclcpp::Subscription::SharedPtr para_sub_; 127 | 128 | NodeExc* node_executor_ = nullptr; 129 | 130 | std::function< void( std::string ) > error_callback_; 131 | bool running_ = false; 132 | bool connected_ = false; 133 | bool want_stop_ = false; 134 | 135 | }; 136 | } 137 | 138 | #endif //_LEGGED_SIMPARAM_HPP__ -------------------------------------------------------------------------------- /cyberdog_gazebo/include/node_executor.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023-2023 Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | #ifndef _NODE_EXCUTOR_HPP__ 15 | #define _NODE_EXCUTOR_HPP__ 16 | 17 | #include "rclcpp/rclcpp.hpp" 18 | 19 | namespace gazebo 20 | { 21 | class GazeboNode: public rclcpp::Node 22 | { 23 | public: 24 | /** 25 | * @brief Construct a new GazeboNode object to subscribe topic messages 26 | * 27 | * @param node_name 28 | */ 29 | GazeboNode(std::string node_name):Node(node_name){}; 30 | ~GazeboNode(){}; 31 | }; 32 | 33 | class NodeExc 34 | { 35 | public: 36 | /** 37 | * @brief Construct a new NodeExecutor object 38 | * 39 | */ 40 | NodeExc(){} 41 | 42 | /** 43 | * @brief Add node into node executor 44 | * 45 | * @param node 46 | */ 47 | void AddNode(std::shared_ptr node) 48 | { 49 | executor_.add_node(node); 50 | } 51 | 52 | /** 53 | * @brief Receive topics 54 | * 55 | */ 56 | void ReceiveTopic() 57 | { 58 | using namespace std::chrono_literals; 59 | auto wait_time = 100ns; 60 | executor_.spin_once(wait_time); 61 | } 62 | 63 | 64 | private: 65 | rclcpp::executors::SingleThreadedExecutor executor_; 66 | }; 67 | 68 | 69 | } 70 | 71 | #endif //_NODE_EXCUTOR_HPP__ -------------------------------------------------------------------------------- /cyberdog_gazebo/launch/cyberdog_control_launch.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python3 2 | # 3 | # Copyright (c) 2023-2023 Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved. 4 | 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | 17 | import os 18 | import sys 19 | 20 | import launch 21 | import subprocess 22 | import launch_ros.actions 23 | from ament_index_python.packages import get_package_prefix 24 | from ament_index_python.packages import get_package_share_directory 25 | # from ament_index_python.packages import get_package_library_directory 26 | 27 | def generate_launch_description(): 28 | # Get the launch directory 29 | library_dir = get_package_share_directory('cyberdog_locomotion') 30 | cmd_path = os.path.join(library_dir, "../../lib/cyberdog_locomotion") 31 | 32 | # cd 33 | # open_cmd = os.path.join("cd ", cmd_path) 34 | open_cmd = "cd " + cmd_path + " && ./cyberdog_control m s" 35 | print(open_cmd) 36 | 37 | os.system(open_cmd) 38 | 39 | ld = launch.LaunchDescription() 40 | return ld 41 | 42 | if __name__ == '__main__': 43 | generate_launch_description() 44 | -------------------------------------------------------------------------------- /cyberdog_gazebo/launch/heightmap_gazebo.launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | from time import sleep 3 | import launch 4 | from launch.conditions import IfCondition, UnlessCondition 5 | from launch.launch_description_sources import PythonLaunchDescriptionSource 6 | from launch.substitutions import Command, LaunchConfiguration, PythonExpression 7 | from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription 8 | from launch.actions import OpaqueFunction 9 | from ament_index_python.packages import get_package_prefix 10 | from ament_index_python.packages import get_package_share_directory 11 | import xacro 12 | 13 | 14 | def launch_setup(context, *args, **kwargs): 15 | # pkg 16 | pkg_share = get_package_share_directory('cyberdog_gazebo') 17 | pkg_prefix = get_package_prefix('cyberdog_gazebo') 18 | pkg_gazebo_ros = get_package_share_directory('gazebo_ros') 19 | 20 | # config 21 | hang_robot = LaunchConfiguration('hang_robot').perform(context) 22 | use_lidar = LaunchConfiguration('use_lidar').perform(context) 23 | wname = LaunchConfiguration('wname').perform(context) 24 | rname = LaunchConfiguration('rname').perform(context) 25 | 26 | # env 27 | my_env = os.environ.copy() 28 | my_env["GAZEBO_MODEL_PATH"] = os.path.join(pkg_share, 'model') 29 | my_env["GAZEBO_PLUGIN_PATH"] = os.path.join(get_package_prefix('cyberdog_gazebo'), 'lib') 30 | 31 | # world 32 | world_path = os.path.join(pkg_share, 'world', wname+'.world') 33 | 34 | # urdf 35 | xacro_path = os.path.join(get_package_share_directory( 36 | rname+'_description'), 'xacro', 'robot.xacro') 37 | urdf_contents = xacro.process_file(xacro_path, mappings={ 38 | 'DEBUG': hang_robot, 'USE_LIDAR': use_lidar}).toprettyxml(indent=' ') 39 | 40 | # spawn 41 | spawn_entity_message_contents = "'{initial_pose:{ position: {x: 0, y: 0, z: 0.6}, orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}}, name: \"robot\", xml: \"" + \ 42 | urdf_contents.replace('"', '\\"') + "\"}'" 43 | spawn_entity = launch.actions.ExecuteProcess( 44 | name='spawn_entity', cmd=['ros2', 'service', 'call', '/spawn_entity', 'gazebo_msgs/SpawnEntity', spawn_entity_message_contents], env=my_env, shell=True, log_cmd=False) 45 | 46 | # gazebo server 47 | start_gazebo_server_cmd = IncludeLaunchDescription( 48 | PythonLaunchDescriptionSource(os.path.join( 49 | pkg_gazebo_ros, 'launch', 'gzserver.launch.py')), 50 | condition=IfCondition(LaunchConfiguration('use_simulator')), 51 | launch_arguments={ 52 | 'world': world_path, 53 | 'debug': LaunchConfiguration('debug'), 54 | 'gui': LaunchConfiguration('gui'), 55 | 'paused': LaunchConfiguration('paused'), 56 | 'use_sim_time': LaunchConfiguration('use_sim_time'), 57 | 'headless': LaunchConfiguration('headless'), 58 | 'env': my_env 59 | }.items() 60 | ) 61 | 62 | # gazebo client 63 | start_gazebo_client_cmd = IncludeLaunchDescription( 64 | PythonLaunchDescriptionSource(os.path.join( 65 | pkg_gazebo_ros, 'launch', 'gzclient.launch.py')), 66 | condition=IfCondition(PythonExpression( 67 | [LaunchConfiguration('use_simulator'), ' and not ', LaunchConfiguration('headless')])) 68 | ) 69 | 70 | return [ 71 | start_gazebo_server_cmd, 72 | start_gazebo_client_cmd, 73 | spawn_entity] 74 | 75 | 76 | def generate_launch_description(): 77 | ld = launch.LaunchDescription([ 78 | DeclareLaunchArgument( 79 | name='headless', 80 | default_value='False', 81 | description='Whether to execute gzclient' 82 | ), 83 | DeclareLaunchArgument( 84 | name='use_simulator', 85 | default_value='True', 86 | description='Whether to start the simulator' 87 | ), 88 | DeclareLaunchArgument( 89 | name='paused', 90 | default_value='true' 91 | ), 92 | DeclareLaunchArgument( 93 | name='use_sim_time', 94 | default_value='true' 95 | ), 96 | DeclareLaunchArgument( 97 | name='gui', 98 | default_value='true' 99 | ), 100 | DeclareLaunchArgument( 101 | name='debug', 102 | default_value='false' 103 | ), 104 | DeclareLaunchArgument( 105 | name='hang_robot', 106 | default_value='false' 107 | ), 108 | DeclareLaunchArgument( 109 | name='use_lidar', 110 | default_value='false' 111 | ), 112 | DeclareLaunchArgument( 113 | name='rname', 114 | default_value='cyberdog' 115 | ), 116 | DeclareLaunchArgument( 117 | name='wname', 118 | default_value='heightmap' 119 | ), 120 | OpaqueFunction(function=launch_setup) 121 | ]) 122 | return ld 123 | -------------------------------------------------------------------------------- /cyberdog_gazebo/media/materials/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ADD_SUBDIRECTORY(textures) 2 | -------------------------------------------------------------------------------- /cyberdog_gazebo/media/materials/textures/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | set (files 2 | beigeWall.jpg 3 | ceiling_tiled.jpg 4 | clouds.jpg 5 | dirt_diffusespecular.png 6 | dirt_normal.png 7 | flat_normal.png 8 | fungus_diffusespecular.png 9 | fungus_normal.png 10 | granite2.jpg 11 | granite.jpg 12 | grass_diffusespecular.png 13 | grass.jpg 14 | grass_normal.png 15 | hardwood_floor.jpg 16 | motorway.jpg 17 | paintedWall.jpg 18 | pioneerBody.jpg 19 | primary.jpg 20 | projection_filter.png 21 | random.png 22 | residential.jpg 23 | secondary.jpg 24 | sidewalk.jpg 25 | steps.jpeg 26 | stereo_projection_pattern_high_res.png 27 | stereo_projection_pattern_high_res_red.png 28 | terrain.png 29 | trunk.jpg 30 | white.bmp 31 | willowMap.png 32 | wood.jpg 33 | WoodPallet.png 34 | ) 35 | 36 | 37 | INSTALL(FILES ${files} DESTINATION ${CMAKE_INSTALL_PREFIX}/share/gazebo-${GAZEBO_VERSION}/media/materials/textures/) 38 | -------------------------------------------------------------------------------- /cyberdog_gazebo/media/materials/textures/WoodPallet.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MiRoboticsLab/cyberdog_simulator/da6383903b9591e15432284e9356d840fd334c66/cyberdog_gazebo/media/materials/textures/WoodPallet.png -------------------------------------------------------------------------------- /cyberdog_gazebo/media/materials/textures/beigeWall.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MiRoboticsLab/cyberdog_simulator/da6383903b9591e15432284e9356d840fd334c66/cyberdog_gazebo/media/materials/textures/beigeWall.jpg -------------------------------------------------------------------------------- /cyberdog_gazebo/media/materials/textures/ceiling_tiled.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MiRoboticsLab/cyberdog_simulator/da6383903b9591e15432284e9356d840fd334c66/cyberdog_gazebo/media/materials/textures/ceiling_tiled.jpg -------------------------------------------------------------------------------- /cyberdog_gazebo/media/materials/textures/clouds.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MiRoboticsLab/cyberdog_simulator/da6383903b9591e15432284e9356d840fd334c66/cyberdog_gazebo/media/materials/textures/clouds.jpg -------------------------------------------------------------------------------- /cyberdog_gazebo/media/materials/textures/dirt_diffusespecular.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MiRoboticsLab/cyberdog_simulator/da6383903b9591e15432284e9356d840fd334c66/cyberdog_gazebo/media/materials/textures/dirt_diffusespecular.png -------------------------------------------------------------------------------- /cyberdog_gazebo/media/materials/textures/dirt_normal.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MiRoboticsLab/cyberdog_simulator/da6383903b9591e15432284e9356d840fd334c66/cyberdog_gazebo/media/materials/textures/dirt_normal.png -------------------------------------------------------------------------------- /cyberdog_gazebo/media/materials/textures/flat_normal.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MiRoboticsLab/cyberdog_simulator/da6383903b9591e15432284e9356d840fd334c66/cyberdog_gazebo/media/materials/textures/flat_normal.png -------------------------------------------------------------------------------- /cyberdog_gazebo/media/materials/textures/fungus_diffusespecular.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MiRoboticsLab/cyberdog_simulator/da6383903b9591e15432284e9356d840fd334c66/cyberdog_gazebo/media/materials/textures/fungus_diffusespecular.png -------------------------------------------------------------------------------- /cyberdog_gazebo/media/materials/textures/fungus_normal.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MiRoboticsLab/cyberdog_simulator/da6383903b9591e15432284e9356d840fd334c66/cyberdog_gazebo/media/materials/textures/fungus_normal.png -------------------------------------------------------------------------------- /cyberdog_gazebo/media/materials/textures/granite.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MiRoboticsLab/cyberdog_simulator/da6383903b9591e15432284e9356d840fd334c66/cyberdog_gazebo/media/materials/textures/granite.jpg -------------------------------------------------------------------------------- /cyberdog_gazebo/media/materials/textures/granite2.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MiRoboticsLab/cyberdog_simulator/da6383903b9591e15432284e9356d840fd334c66/cyberdog_gazebo/media/materials/textures/granite2.jpg -------------------------------------------------------------------------------- /cyberdog_gazebo/media/materials/textures/grass.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MiRoboticsLab/cyberdog_simulator/da6383903b9591e15432284e9356d840fd334c66/cyberdog_gazebo/media/materials/textures/grass.jpg -------------------------------------------------------------------------------- /cyberdog_gazebo/media/materials/textures/grass_diffusespecular.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MiRoboticsLab/cyberdog_simulator/da6383903b9591e15432284e9356d840fd334c66/cyberdog_gazebo/media/materials/textures/grass_diffusespecular.png -------------------------------------------------------------------------------- /cyberdog_gazebo/media/materials/textures/grass_normal.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MiRoboticsLab/cyberdog_simulator/da6383903b9591e15432284e9356d840fd334c66/cyberdog_gazebo/media/materials/textures/grass_normal.png -------------------------------------------------------------------------------- /cyberdog_gazebo/media/materials/textures/hardwood_floor.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MiRoboticsLab/cyberdog_simulator/da6383903b9591e15432284e9356d840fd334c66/cyberdog_gazebo/media/materials/textures/hardwood_floor.jpg -------------------------------------------------------------------------------- /cyberdog_gazebo/media/materials/textures/motorway.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MiRoboticsLab/cyberdog_simulator/da6383903b9591e15432284e9356d840fd334c66/cyberdog_gazebo/media/materials/textures/motorway.jpg -------------------------------------------------------------------------------- /cyberdog_gazebo/media/materials/textures/paintedWall.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MiRoboticsLab/cyberdog_simulator/da6383903b9591e15432284e9356d840fd334c66/cyberdog_gazebo/media/materials/textures/paintedWall.jpg -------------------------------------------------------------------------------- /cyberdog_gazebo/media/materials/textures/pioneerBody.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MiRoboticsLab/cyberdog_simulator/da6383903b9591e15432284e9356d840fd334c66/cyberdog_gazebo/media/materials/textures/pioneerBody.jpg -------------------------------------------------------------------------------- /cyberdog_gazebo/media/materials/textures/primary.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MiRoboticsLab/cyberdog_simulator/da6383903b9591e15432284e9356d840fd334c66/cyberdog_gazebo/media/materials/textures/primary.jpg -------------------------------------------------------------------------------- /cyberdog_gazebo/media/materials/textures/projection_filter.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MiRoboticsLab/cyberdog_simulator/da6383903b9591e15432284e9356d840fd334c66/cyberdog_gazebo/media/materials/textures/projection_filter.png -------------------------------------------------------------------------------- /cyberdog_gazebo/media/materials/textures/random.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MiRoboticsLab/cyberdog_simulator/da6383903b9591e15432284e9356d840fd334c66/cyberdog_gazebo/media/materials/textures/random.png -------------------------------------------------------------------------------- /cyberdog_gazebo/media/materials/textures/residential.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MiRoboticsLab/cyberdog_simulator/da6383903b9591e15432284e9356d840fd334c66/cyberdog_gazebo/media/materials/textures/residential.jpg -------------------------------------------------------------------------------- /cyberdog_gazebo/media/materials/textures/secondary.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MiRoboticsLab/cyberdog_simulator/da6383903b9591e15432284e9356d840fd334c66/cyberdog_gazebo/media/materials/textures/secondary.jpg -------------------------------------------------------------------------------- /cyberdog_gazebo/media/materials/textures/sidewalk.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MiRoboticsLab/cyberdog_simulator/da6383903b9591e15432284e9356d840fd334c66/cyberdog_gazebo/media/materials/textures/sidewalk.jpg -------------------------------------------------------------------------------- /cyberdog_gazebo/media/materials/textures/steps.jpeg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MiRoboticsLab/cyberdog_simulator/da6383903b9591e15432284e9356d840fd334c66/cyberdog_gazebo/media/materials/textures/steps.jpeg -------------------------------------------------------------------------------- /cyberdog_gazebo/media/materials/textures/stereo_projection_pattern_high_res.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MiRoboticsLab/cyberdog_simulator/da6383903b9591e15432284e9356d840fd334c66/cyberdog_gazebo/media/materials/textures/stereo_projection_pattern_high_res.png -------------------------------------------------------------------------------- /cyberdog_gazebo/media/materials/textures/stereo_projection_pattern_high_res_red.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MiRoboticsLab/cyberdog_simulator/da6383903b9591e15432284e9356d840fd334c66/cyberdog_gazebo/media/materials/textures/stereo_projection_pattern_high_res_red.png -------------------------------------------------------------------------------- /cyberdog_gazebo/media/materials/textures/terrain.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MiRoboticsLab/cyberdog_simulator/da6383903b9591e15432284e9356d840fd334c66/cyberdog_gazebo/media/materials/textures/terrain.png -------------------------------------------------------------------------------- /cyberdog_gazebo/media/materials/textures/texture_information.txt: -------------------------------------------------------------------------------- 1 | Textures for roads were acquired from http://texturelib.com/. 2 | -------------------------------------------------------------------------------- /cyberdog_gazebo/media/materials/textures/trunk.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MiRoboticsLab/cyberdog_simulator/da6383903b9591e15432284e9356d840fd334c66/cyberdog_gazebo/media/materials/textures/trunk.jpg -------------------------------------------------------------------------------- /cyberdog_gazebo/media/materials/textures/white.bmp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MiRoboticsLab/cyberdog_simulator/da6383903b9591e15432284e9356d840fd334c66/cyberdog_gazebo/media/materials/textures/white.bmp -------------------------------------------------------------------------------- /cyberdog_gazebo/media/materials/textures/willowMap.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MiRoboticsLab/cyberdog_simulator/da6383903b9591e15432284e9356d840fd334c66/cyberdog_gazebo/media/materials/textures/willowMap.png -------------------------------------------------------------------------------- /cyberdog_gazebo/media/materials/textures/wood.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MiRoboticsLab/cyberdog_simulator/da6383903b9591e15432284e9356d840fd334c66/cyberdog_gazebo/media/materials/textures/wood.jpg -------------------------------------------------------------------------------- /cyberdog_gazebo/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | cyberdog_gazebo 5 | 0.0.0 6 | The cyberdog_gazebo package 7 | 8 | ljh 9 | You Yangwei 10 | 11 | 12 | 13 | 14 | Apache License, Version 2.0 15 | 16 | ament_cmake 17 | 18 | cyberdog_msg 19 | 20 | rclcpp 21 | gazebo_msgs 22 | gazebo_dev 23 | gazebo_ros 24 | yaml_cpp_vendor 25 | 26 | 27 | ament_cmake 28 | 29 | 30 | -------------------------------------------------------------------------------- /cyberdog_gazebo/script/gazebolauncher.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # Copyright (c) 2023-2023 Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved. 3 | 4 | # Licensed under the Apache License, Version 2.0 (the "License"); 5 | # you may not use this file except in compliance with the License. 6 | # You may obtain a copy of the License at 7 | 8 | # http://www.apache.org/licenses/LICENSE-2.0 9 | 10 | # Unless required by applicable law or agreed to in writing, software 11 | # distributed under the License is distributed on an "AS IS" BASIS, 12 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | # See the License for the specific language governing permissions and 14 | # limitations under the License. 15 | 16 | import sys, signal, subprocess, time 17 | 18 | 19 | timeout_before_kill = 1.0 # [s] 20 | timeout_after_kill = 1.0 # [s] 21 | 22 | 23 | def signal_handler(sig, frame): 24 | print('killing gazebo') 25 | time.sleep(timeout_before_kill) 26 | subprocess.call("killall -q gzclient & killall -q gzserver", shell=True) 27 | time.sleep(timeout_after_kill) 28 | subprocess.call("killall -9 -q gzclient & killall -9 -q gzserver", shell=True) 29 | sys.exit(0) 30 | 31 | 32 | if __name__ == "__main__": 33 | signal.signal(signal.SIGINT, signal_handler) 34 | cmd = ' '.join(sys.argv[1:]) 35 | print(cmd) 36 | subprocess.call(cmd, shell=True) -------------------------------------------------------------------------------- /cyberdog_gazebo/script/launchcontrol.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | source /opt/ros/galactic/setup.bash 4 | source install/setup.bash 5 | 6 | ros2 launch cyberdog_gazebo cyberdog_control_launch.py -------------------------------------------------------------------------------- /cyberdog_gazebo/script/launchgazebo.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | source /opt/ros/galactic/setup.bash 4 | source install/setup.bash 5 | chmod +x src/cyberdog_simulator/cyberdog_gazebo/script/gazebolauncher.py 6 | 7 | # ros2 launch cyberdog_gazebo gazebo.launch.py 8 | python3 src/cyberdog_simulator/cyberdog_gazebo/script/gazebolauncher.py ros2 launch cyberdog_gazebo gazebo.launch.py -------------------------------------------------------------------------------- /cyberdog_gazebo/script/launchsim.py: -------------------------------------------------------------------------------- 1 | #!usr/bin/env python 2 | 3 | # Copyright (c) 2023-2023 Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved. 4 | 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | 17 | import os 18 | import sys 19 | import time 20 | 21 | def launchsim(): 22 | 23 | os.system('gnome-terminal -t "cyberdog_gazebo" -e "bash ./src/cyberdog_simulator/cyberdog_gazebo/script/launchgazebo.sh"') 24 | time.sleep(5) 25 | os.system('gnome-terminal -t "cyberdog_viusal" -e "bash ./src/cyberdog_simulator/cyberdog_gazebo/script/launchvisual.sh"') 26 | os.system('gnome-terminal -t "cyberdog_control" -e "bash ./src/cyberdog_simulator/cyberdog_gazebo/script/launchcontrol.sh"') 27 | 28 | if __name__=="__main__": 29 | launchsim() 30 | -------------------------------------------------------------------------------- /cyberdog_gazebo/script/launchvisual.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # gnome-terminal -t "cyberdog_gazebo" -x bash -c "source /opt/ros/galactic/setup.bash;source install/setup.bash;ros2 launch cyberdog_gazebo gazebo.launch.py rname:=l91_p1_1;exec bash;" 4 | source /opt/ros/galactic/setup.bash 5 | source install/setup.bash 6 | 7 | ros2 launch cyberdog_visual cyberdog_visual.launch.py -------------------------------------------------------------------------------- /cyberdog_gazebo/src/control_parameters/control_parameter_interface.cpp: -------------------------------------------------------------------------------- 1 | /*! @file ControlParameterInterface.cpp 2 | * @brief Types to allow remote access of control parameters, for use with 3 | * LCM/Shared memory 4 | * 5 | * There are response/request messages. The robot receives request messages and 6 | * responds with response messages The request messages either request setting a 7 | * parameter or getting a parameter 8 | */ 9 | 10 | #include "control_parameters/control_parameter_interface.hpp" 11 | 12 | /*! 13 | * Convert control parameter request kind into a string 14 | */ 15 | std::string ControlParameterRequestKindToString( ControlParameterRequestKind request ) { 16 | switch ( request ) { 17 | case ControlParameterRequestKind::kGET_USER_PARAM_BY_NAME: 18 | return "get user"; 19 | case ControlParameterRequestKind::kSET_USER_PARAM_BY_NAME: 20 | return "set user"; 21 | case ControlParameterRequestKind::kGET_ROBOT_PARAM_BY_NAME: 22 | return "get robot"; 23 | case ControlParameterRequestKind::kSET_ROBOT_PARAM_BY_NAME: 24 | return "set robot"; 25 | case ControlParameterRequestKind::kGET_SPEED_CALIBRATE_PARAM_BY_NAME: 26 | return "get speed_calibrate"; 27 | case ControlParameterRequestKind::kSET_SPEED_CALIBRATE_PARAM_BY_NAME: 28 | return "set speed_calibrate"; 29 | case ControlParameterRequestKind::kGET_IMU_CALIBRATE_PARAM_BY_NAME: 30 | return "get imu_calibrate"; 31 | case ControlParameterRequestKind::kSET_IMU_CALIBRATE_PARAM_BY_NAME: 32 | return "set imu_calibrate"; 33 | default: 34 | return "unknown"; 35 | } 36 | } 37 | -------------------------------------------------------------------------------- /cyberdog_gazebo/src/foot_contact_plugin.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023-2023 Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "foot_contact_plugin.hpp" 16 | 17 | namespace gazebo 18 | { 19 | 20 | GZ_REGISTER_SENSOR_PLUGIN(FootContactPlugin) 21 | 22 | ///////////////////////////////////////////////// 23 | FootContactPlugin::FootContactPlugin() : SensorPlugin() 24 | { 25 | } 26 | 27 | ///////////////////////////////////////////////// 28 | FootContactPlugin::~FootContactPlugin() 29 | { 30 | } 31 | 32 | ///////////////////////////////////////////////// 33 | void FootContactPlugin::Load(sensors::SensorPtr _sensor, sdf::ElementPtr /*_sdf*/) 34 | { 35 | // Get the parent sensor. 36 | this->parentSensor = 37 | std::dynamic_pointer_cast(_sensor); 38 | 39 | // Make sure the parent sensor is valid. 40 | if (!this->parentSensor) 41 | { 42 | gzerr << "FootContactPlugin requires a ContactSensor.\n"; 43 | return; 44 | } 45 | 46 | // Connect to the sensor update event. 47 | this->updateConnection = this->parentSensor->ConnectUpdated( 48 | std::bind(&FootContactPlugin::OnUpdate, this)); 49 | 50 | // Make sure the parent sensor is active. 51 | this->parentSensor->SetActive(true); 52 | } 53 | 54 | ///////////////////////////////////////////////// 55 | void FootContactPlugin::OnUpdate() {} 56 | 57 | 58 | 59 | } -------------------------------------------------------------------------------- /cyberdog_gazebo/src/lcmhandler.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023-2023 Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "lcmhandler.hpp" 16 | 17 | namespace gazebo 18 | { 19 | LCMHandler::LCMHandler(){ 20 | if (!lcm_.good()){ 21 | exit(1); 22 | } 23 | 24 | lcm_.subscribe("gamepad_lcmt", &LCMHandler::HandleCommand, this); 25 | 26 | } 27 | 28 | GamepadCommand LCMHandler::ReceiveGPC() 29 | { 30 | lcm_.handle(); 31 | gamepad_command_.a=lcm_gamepad_.a; 32 | gamepad_command_.b=lcm_gamepad_.b; 33 | gamepad_command_.x=lcm_gamepad_.x; 34 | gamepad_command_.y=lcm_gamepad_.y; 35 | gamepad_command_.leftStickAnalog[0]=lcm_gamepad_.leftStickAnalog[0]; 36 | gamepad_command_.leftStickAnalog[1]=lcm_gamepad_.leftStickAnalog[1]; 37 | gamepad_command_.rightStickAnalog[0]=lcm_gamepad_.rightStickAnalog[0]; 38 | gamepad_command_.rightStickAnalog[1]=lcm_gamepad_.rightStickAnalog[1]; 39 | 40 | return gamepad_command_; 41 | } 42 | 43 | void LCMHandler::HandleCommand(const lcm::ReceiveBuffer *rbuf, const std::string &chan, const gamepad_lcmt *msg) 44 | { 45 | lcm_gamepad_ = *msg; 46 | } 47 | 48 | bool LCMHandler::HasEvent() 49 | { 50 | // setup the LCM file descriptor for waiting. 51 | int lcm_fd = lcm_.getFileno(); 52 | fd_set fds; 53 | FD_ZERO(&fds); 54 | FD_SET(lcm_fd, &fds); 55 | 56 | // wait a limited amount of time for an incoming message 57 | struct timeval timeout = { 58 | 0, // seconds 59 | 1 // microseconds 60 | }; 61 | int status = select(lcm_fd + 1, &fds, 0, 0, &timeout); 62 | 63 | if (0 == status) { 64 | // no messages 65 | return false; 66 | } 67 | else if (FD_ISSET(lcm_fd, &fds)) { 68 | // LCM has events ready to be processed. 69 | return true; 70 | } 71 | else{ 72 | // Undefined 73 | return false; 74 | } 75 | } 76 | 77 | void LCMHandler::SendSimData(simulator_lcmt &_lcm_sim_handler) 78 | { 79 | lcm_.publish("simulator_state", &_lcm_sim_handler); 80 | } 81 | 82 | } -------------------------------------------------------------------------------- /cyberdog_gazebo/src/sim_utilities/spine_board.cpp: -------------------------------------------------------------------------------- 1 | /*! @file spine_board.cpp 2 | * @brief Spine Board Code, used to simulate the SpineBoard. 3 | */ 4 | 5 | #include 6 | 7 | #include "sim_utilities/spine_board.hpp" 8 | 9 | /*! 10 | * Spine board setup (per board) 11 | */ 12 | void SpineBoard::init(float sideSign, s32 board) { 13 | this->board_num = board; 14 | this->side_sign = sideSign; 15 | } 16 | 17 | /*! 18 | * Reset all data for the board 19 | */ 20 | void SpineBoard::resetData() { 21 | if (data == nullptr) { 22 | printf( 23 | "[ERROR: SPINE board] reset_spine_board_data called when " 24 | "cheetahlcm_spi_data_t* was null\n"); 25 | return; 26 | } 27 | 28 | data->flags[board_num] = 0; 29 | data->qd_abad[board_num] = 0.f; 30 | data->qd_hip[board_num] = 0.f; 31 | data->qd_knee[board_num] = 0.f; 32 | data->q_abad[board_num] = 0.f; 33 | data->q_hip[board_num] = 0.f; 34 | data->q_knee[board_num] = 0.f; 35 | data->spi_driver_status = 0; 36 | } 37 | 38 | /*! 39 | * Reset all commands for the board 40 | */ 41 | void SpineBoard::resetCommand() { 42 | if (cmd == nullptr) { 43 | printf( 44 | "[ERROR: SPINE board] reset_spine_board_command called when " 45 | "cheetahlcm_spi_command_t* was null\n"); 46 | return; 47 | } 48 | 49 | cmd->flags[board_num] = 0; 50 | cmd->kd_abad[board_num] = 0.f; 51 | cmd->kd_hip[board_num] = 0.f; 52 | cmd->kd_knee[board_num] = 0.f; 53 | cmd->kp_abad[board_num] = 0.f; 54 | cmd->kp_hip[board_num] = 0.f; 55 | cmd->kp_knee[board_num] = 0.f; 56 | cmd->qd_des_abad[board_num] = 0.f; 57 | cmd->qd_des_hip[board_num] = 0.f; 58 | cmd->qd_des_knee[board_num] = 0.f; 59 | cmd->q_des_abad[board_num] = 0.f; 60 | cmd->q_des_hip[board_num] = 0.f; 61 | cmd->q_des_knee[board_num] = 0.f; 62 | cmd->tau_abad_ff[board_num] = 0.f; 63 | cmd->tau_hip_ff[board_num] = 0.f; 64 | cmd->tau_hip_ff[board_num] = 0.f; 65 | cmd->tau_knee_ff[board_num] = 0.f; 66 | } 67 | 68 | /*! 69 | * Run spine board control 70 | */ 71 | void SpineBoard::run() { 72 | iter_counter++; 73 | if (cmd == nullptr || data == nullptr) { 74 | printf( 75 | "[ERROR: SPINE board] run_spine_board_iteration called with null " 76 | "command or data!\n"); 77 | torque_out[0] = 0.f; 78 | torque_out[1] = 0.f; 79 | torque_out[2] = 0.f; 80 | return; 81 | } 82 | 83 | /// Check abad softstop /// 84 | if (data->q_abad[board_num] > q_limit_p[0]) { 85 | torque_out[0] = kp_softstop * (q_limit_p[0] - data->q_abad[board_num]) - 86 | kd_softstop * (data->qd_abad[board_num]) + 87 | cmd->tau_abad_ff[board_num]; 88 | } else if (data->q_abad[board_num] < q_limit_n[0]) { 89 | torque_out[0] = kp_softstop * (q_limit_n[0] - data->q_abad[board_num]) - 90 | kd_softstop * (data->qd_abad[board_num]) + 91 | cmd->tau_abad_ff[board_num]; 92 | } else { 93 | torque_out[0] = cmd->kp_abad[board_num] * 94 | (cmd->q_des_abad[board_num] - data->q_abad[board_num]) + 95 | cmd->kd_abad[board_num] * (cmd->qd_des_abad[board_num] - 96 | data->qd_abad[board_num]) + 97 | cmd->tau_abad_ff[board_num]; 98 | } 99 | 100 | /// Check hip softstop /// 101 | if (data->q_hip[board_num] > q_limit_p[1]) { 102 | torque_out[1] = kp_softstop * (q_limit_p[1] - data->q_hip[board_num]) - 103 | kd_softstop * (data->qd_hip[board_num]) + 104 | cmd->tau_hip_ff[board_num]; 105 | } else if (data->q_hip[board_num] < q_limit_n[1]) { 106 | torque_out[1] = kp_softstop * (q_limit_n[1] - data->q_hip[board_num]) - 107 | kd_softstop * (data->qd_hip[board_num]) + 108 | cmd->tau_hip_ff[board_num]; 109 | } else { 110 | torque_out[1] = cmd->kp_hip[board_num] * 111 | (cmd->q_des_hip[board_num] - data->q_hip[board_num]) + 112 | cmd->kd_hip[board_num] * 113 | (cmd->qd_des_hip[board_num] - data->qd_hip[board_num]) + 114 | cmd->tau_hip_ff[board_num]; 115 | } 116 | 117 | /// No knee softstop right now /// 118 | torque_out[2] = cmd->kp_knee[board_num] * 119 | (cmd->q_des_knee[board_num] - data->q_knee[board_num]) + 120 | cmd->kd_knee[board_num] * 121 | (cmd->qd_des_knee[board_num] - data->qd_knee[board_num]) + 122 | cmd->tau_knee_ff[board_num]; 123 | 124 | const float* torque_limits = disabled_torque; 125 | 126 | if (cmd->flags[board_num] & 0b1) { 127 | if (cmd->flags[board_num] & 0b10) 128 | torque_limits = wimp_torque; 129 | else 130 | torque_limits = max_torque; 131 | } 132 | 133 | for (int i = 0; i < 3; i++) { 134 | if (torque_out[i] > torque_limits[i]) torque_out[i] = torque_limits[i]; 135 | if (torque_out[i] < -torque_limits[i]) torque_out[i] = -torque_limits[i]; 136 | } 137 | 138 | } 139 | -------------------------------------------------------------------------------- /cyberdog_gazebo/src/utilities/utilities.cpp: -------------------------------------------------------------------------------- 1 | /*! 2 | * @file utilities.cpp 3 | * @brief Common utility functions 4 | */ 5 | 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | #include "utilities/utilities.h" 12 | #include "Configuration.h" 13 | 14 | 15 | /** 16 | * @brief Write std::string to file with given name 17 | * 18 | * @param fileName 19 | * @param fileData 20 | */ 21 | void WriteStringToFile( const std::string& fileName, const std::string& fileData ) { 22 | FILE* fp = fopen( fileName.c_str(), "w" ); 23 | if ( !fp ) { 24 | printf( "Failed to fopen %s\n", fileName.c_str() ); 25 | throw std::runtime_error( "Failed to open file" ); 26 | } 27 | fprintf( fp, "%s", fileData.c_str() ); 28 | fclose( fp ); 29 | } 30 | 31 | /** 32 | * @brief Get the current time and return date object as a string 33 | * 34 | * @return std::string 35 | */ 36 | std::string GetCurrentTimeAndDate() { 37 | auto t = std::time( nullptr ); 38 | auto tm = *std::localtime( &t ); 39 | std::ostringstream ss; 40 | ss << std::put_time( &tm, "%c" ); 41 | return ss.str(); 42 | } 43 | 44 | /** 45 | * @brief Return the path of config directory 46 | * 47 | * @return std::string 48 | */ 49 | std::string GetConfigDirectoryPath() { 50 | return std::string( THIS_COM ) + "config/"; 51 | } 52 | 53 | /** 54 | * @brief Return the path of config directory of control program 55 | * 56 | * @return std::string 57 | */ 58 | std::string GetLocoConfigDirectoryPath() { return std::string( THIS_COM ) + "../../cyberdog_locomotion/common/config/"; } 59 | -------------------------------------------------------------------------------- /cyberdog_gazebo/third-party/ParamHandler/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | 2 | 3 | find_package(yaml_cpp_vendor REQUIRED) 4 | 5 | 6 | option(PARAM_TEST "Build test programe for ParamHandler") 7 | 8 | 9 | set(YAML_CPP_INCLUDE_DIR "/opt/ros/galactic/opt/yaml_cpp_vendor/include") 10 | set(YAML_CPP_LIBRARIES "/opt/ros/galactic/opt/yaml_cpp_vendor/lib/libyaml-cpp.so") 11 | 12 | include_directories(${YAML_CPP_INCLUDE_DIR}) 13 | 14 | 15 | 16 | 17 | ### 18 | ### Library 19 | ### 20 | add_library(param_handler SHARED 21 | ParamHandler.hpp 22 | ParamHandler.cpp 23 | ) 24 | 25 | 26 | 27 | 28 | target_include_directories(param_handler PUBLIC 29 | ${YAML_CPP_INCLUDE_DIR} 30 | ) 31 | 32 | target_link_libraries(param_handler 33 | ${YAML_CPP_LIBRARIES} 34 | ) 35 | 36 | if (PARAM_TEST) 37 | add_executable(test_param 38 | test/test_param_main.cpp 39 | ) 40 | 41 | target_link_libraries(test_param 42 | param_handler 43 | ) 44 | endif() 45 | -------------------------------------------------------------------------------- /cyberdog_gazebo/third-party/ParamHandler/LICENSE.txt: -------------------------------------------------------------------------------- 1 | Copyright (c) 2008-2015 Jesse Beder. 2 | 3 | Permission is hereby granted, free of charge, to any person obtaining a copy 4 | of this software and associated documentation files (the "Software"), to deal 5 | in the Software without restriction, including without limitation the rights 6 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 7 | copies of the Software, and to permit persons to whom the Software is 8 | furnished to do so, subject to the following conditions: 9 | 10 | The above copyright notice and this permission notice shall be included in 11 | all copies or substantial portions of the Software. 12 | 13 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 14 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 15 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 16 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 17 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 18 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 19 | THE SOFTWARE. 20 | -------------------------------------------------------------------------------- /cyberdog_gazebo/third-party/ParamHandler/ParamHandler.cpp: -------------------------------------------------------------------------------- 1 | #include "ParamHandler.hpp" 2 | #include 3 | 4 | ParamHandler::ParamHandler(const std::string &file_name) { 5 | try { 6 | config_ = YAML::LoadFile(file_name); 7 | fileLoaded = true; 8 | } catch (std::exception& e) { 9 | fileLoaded = false; 10 | } 11 | } 12 | 13 | ParamHandler::~ParamHandler() {} 14 | 15 | bool ParamHandler::getString(const std::string &key, std::string &str_value) { 16 | try { 17 | str_value = config_[key].as(); 18 | } catch (std::exception &e) { 19 | return false; 20 | } 21 | return true; 22 | } 23 | 24 | bool ParamHandler::getString(const std::string &category, const std::string &key, std::string &str_value) { 25 | try { 26 | str_value = config_[category][key].as(); 27 | } catch(std::exception &e) { 28 | return false; 29 | } 30 | return true; 31 | } 32 | 33 | bool ParamHandler::getBoolean(const std::string &key, bool &bool_value) { 34 | try { 35 | bool_value = config_[key].as(); 36 | } catch (std::exception &e) { 37 | return false; 38 | } 39 | return true; 40 | } 41 | 42 | bool ParamHandler::getInteger(const std::string &key, int &int_value) { 43 | try { 44 | int_value = config_[key].as(); 45 | } catch (std::exception &e) { 46 | return false; 47 | } 48 | return true; 49 | } 50 | -------------------------------------------------------------------------------- /cyberdog_gazebo/third-party/ParamHandler/ParamHandler.hpp: -------------------------------------------------------------------------------- 1 | #ifndef PARAMETER_HANDLER 2 | #define PARAMETER_HANDLER 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | using YAML::NodeType; 9 | 10 | class ParamHandler { 11 | public: 12 | ParamHandler(const std::string &file_name); 13 | 14 | virtual ~ParamHandler(); 15 | 16 | bool getString(const std::string &key, std::string &str_value); 17 | bool getString(const std::string &category, const std::string &key, std::string & str_value); 18 | 19 | NodeType::value getType(const std::string& key) { 20 | return config_[key].Type(); 21 | } 22 | 23 | template 24 | bool getVector(const std::string &key, std::vector &vec_value) { 25 | try { 26 | vec_value = config_[key].as >(); 27 | } catch (std::exception &e) { 28 | return false; 29 | } 30 | return true; 31 | } 32 | 33 | template 34 | bool getVector(const std::string &category, const std::string &key, std::vector &vec_value) { 35 | try { 36 | vec_value = config_[category][key].as>(); 37 | } catch (std::exception &e) { 38 | return false; 39 | } 40 | return true; 41 | } 42 | 43 | template 44 | bool get2DArray(const std::string &category, const std::string &key, std::vector > &vec_value) { 45 | try { 46 | vec_value = config_[category][key].as > >(); 47 | } catch (std::exception &e) { 48 | return false; 49 | } 50 | return true; 51 | } 52 | 53 | template 54 | bool get2DArray(const std::string &key, std::vector > &vec_value) { 55 | try { 56 | vec_value = config_[key].as > >(); 57 | } catch (std::exception &e) { 58 | return false; 59 | } 60 | return true; 61 | } 62 | 63 | template 64 | bool getValue(const std::string &key, T &T_value) { 65 | try { 66 | T_value = config_[key].as(); 67 | } catch (std::exception &e) { 68 | return false; 69 | } 70 | return true; 71 | } 72 | 73 | template 74 | bool getValue(const std::string &category, const std::string &key, T &T_value) { 75 | try { 76 | T_value = config_[category][key].as(); 77 | return true; 78 | } catch (std::exception &e) { 79 | return false; 80 | } 81 | return true; 82 | } 83 | 84 | bool getBoolean(const std::string & category, const std::string &key, bool &bool_value){ 85 | try { 86 | bool_value = config_[category][key].as(); 87 | return true; 88 | } catch (std::exception &e) { 89 | return false; 90 | } 91 | return true; 92 | } 93 | 94 | 95 | std::vector getKeys() { 96 | std::vector v; 97 | v.reserve(config_.size()); 98 | for(auto it = config_.begin(); it != config_.end(); it++) { 99 | v.push_back(it->first.as()); 100 | } 101 | return v; 102 | } 103 | 104 | 105 | bool getBoolean(const std::string &key, bool &bool_value); 106 | 107 | bool getInteger(const std::string &key, int &int_value); 108 | 109 | bool fileOpenedSuccessfully() { 110 | return fileLoaded; 111 | } 112 | 113 | protected: 114 | YAML::Node config_; 115 | 116 | private: 117 | bool fileLoaded = false; 118 | }; 119 | 120 | #endif 121 | -------------------------------------------------------------------------------- /cyberdog_gazebo/third-party/ParamHandler/README.md: -------------------------------------------------------------------------------- 1 | # yaml-cpp [![Build Status](https://travis-ci.org/jbeder/yaml-cpp.svg?branch=master)](https://travis-ci.org/jbeder/yaml-cpp) [![Documentation](https://codedocs.xyz/jbeder/yaml-cpp.svg)](https://codedocs.xyz/jbeder/yaml-cpp/) 2 | 3 | yaml-cpp is a [YAML](http://www.yaml.org/) parser and emitter in C++ matching the [YAML 1.2 spec](http://www.yaml.org/spec/1.2/spec.html). 4 | 5 | To get a feel for how it can be used, see the [Tutorial](https://github.com/jbeder/yaml-cpp/wiki/Tutorial) or [How to Emit YAML](https://github.com/jbeder/yaml-cpp/wiki/How-To-Emit-YAML). For the old API (version < 0.5.0), see [How To Parse A Document](https://github.com/jbeder/yaml-cpp/wiki/How-To-Parse-A-Document-(Old-API)). 6 | 7 | # Problems? # 8 | 9 | If you find a bug, post an [issue](https://github.com/jbeder/yaml-cpp/issues)! If you have questions about how to use yaml-cpp, please post it on http://stackoverflow.com and tag it [`yaml-cpp`](http://stackoverflow.com/questions/tagged/yaml-cpp). 10 | 11 | # How to Build # 12 | 13 | yaml-cpp uses [CMake](http://www.cmake.org) to support cross-platform building. The basic steps to build are: 14 | 15 | 1. Download and install [CMake](http://www.cmake.org) (Resources -> Download). 16 | 17 | **Note:** If you don't use the provided installer for your platform, make sure that you add CMake's bin folder to your path. 18 | 19 | 2. Navigate into the source directory, and type: 20 | 21 | ``` 22 | mkdir build 23 | cd build 24 | ``` 25 | 26 | 3. Run CMake. The basic syntax is: 27 | 28 | ``` 29 | cmake [-G generator] [-DBUILD_SHARED_LIBS=ON|OFF] .. 30 | ``` 31 | 32 | * The `generator` is whatever type of build system you'd like to use. To see a full list of generators on your platform, just run `cmake` (with no arguments). For example: 33 | * On Windows, you might use "Visual Studio 12 2013" to generate a Visual Studio 2013 solution or "Visual Studio 14 2015 Win64" to generate a 64-bit Visual Studio 2015 solution. 34 | * On OS X, you might use "Xcode" to generate an Xcode project 35 | * On a UNIX-y system, simply omit the option to generate a makefile 36 | 37 | * yaml-cpp defaults to building a static library, but you may build a shared library by specifying `-DBUILD_SHARED_LIBS=ON`. 38 | 39 | * For more options on customizing the build, see the [CMakeLists.txt](https://github.com/jbeder/yaml-cpp/blob/master/CMakeLists.txt) file. 40 | 41 | 4. Build it! 42 | 43 | 5. To clean up, just remove the `build` directory. 44 | 45 | # Recent Release # 46 | 47 | [yaml-cpp 0.5.3](https://github.com/jbeder/yaml-cpp/releases/tag/release-0.5.3) has been released! This is a bug fix release. It also will be the last release that uses Boost; futures releases will require C++11 instead. 48 | 49 | [yaml-cpp 0.3.0](https://github.com/jbeder/yaml-cpp/releases/tag/release-0.3.0) is still available if you want the old API. 50 | 51 | **The old API will continue to be supported, and will still receive bugfixes!** The 0.3.x and 0.4.x versions will be old API releases, and 0.5.x and above will all be new API releases. 52 | -------------------------------------------------------------------------------- /cyberdog_gazebo/third-party/ParamHandler/param_helper.hpp: -------------------------------------------------------------------------------- 1 | /*! 2 | * @file param_helper.hpp 3 | * @brief some helper function for parameter parser 4 | * 5 | * 6 | * Authored by JackeyHuo, 2021-02-07 7 | */ 8 | 9 | #ifndef PARAM_HELPER_H 10 | #define PARAM_HELPER_H 11 | 12 | #include 13 | #include 14 | #include 15 | 16 | 17 | template 18 | std::vector loadVectorFromString(const std::string& str_buff) { 19 | std::vector vv; 20 | try { 21 | YAML::Node node = YAML::Load(str_buff); 22 | vv = node.as>(); 23 | } catch (std::exception& e) { 24 | // do nothing 25 | } 26 | return vv; 27 | } 28 | 29 | template 30 | std::vector> loadMatFromString(const std::string& str_buff) { 31 | std::vector> mm; 32 | try { 33 | YAML::Node node = YAML::Load(str_buff); 34 | mm = node.as>>(); 35 | } catch (std::exception& e) { 36 | // do nothing 37 | } 38 | return mm; 39 | } 40 | 41 | 42 | #endif // PARAM_HELPER_H 43 | -------------------------------------------------------------------------------- /cyberdog_gazebo/third-party/ParamHandler/src/binary.cpp: -------------------------------------------------------------------------------- 1 | #include "dynacore_yaml-cpp/binary.h" 2 | 3 | namespace dynacore_YAML { 4 | static const char encoding[] = 5 | "ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz0123456789+/"; 6 | 7 | std::string EncodeBase64(const unsigned char *data, std::size_t size) { 8 | const char PAD = '='; 9 | 10 | std::string ret; 11 | ret.resize(4 * size / 3 + 3); 12 | char *out = &ret[0]; 13 | 14 | std::size_t chunks = size / 3; 15 | std::size_t remainder = size % 3; 16 | 17 | for (std::size_t i = 0; i < chunks; i++, data += 3) { 18 | *out++ = encoding[data[0] >> 2]; 19 | *out++ = encoding[((data[0] & 0x3) << 4) | (data[1] >> 4)]; 20 | *out++ = encoding[((data[1] & 0xf) << 2) | (data[2] >> 6)]; 21 | *out++ = encoding[data[2] & 0x3f]; 22 | } 23 | 24 | switch (remainder) { 25 | case 0: 26 | break; 27 | case 1: 28 | *out++ = encoding[data[0] >> 2]; 29 | *out++ = encoding[((data[0] & 0x3) << 4)]; 30 | *out++ = PAD; 31 | *out++ = PAD; 32 | break; 33 | case 2: 34 | *out++ = encoding[data[0] >> 2]; 35 | *out++ = encoding[((data[0] & 0x3) << 4) | (data[1] >> 4)]; 36 | *out++ = encoding[((data[1] & 0xf) << 2)]; 37 | *out++ = PAD; 38 | break; 39 | } 40 | 41 | ret.resize(out - &ret[0]); 42 | return ret; 43 | } 44 | 45 | static const unsigned char decoding[] = { 46 | 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 47 | 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 48 | 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 62, 255, 49 | 255, 255, 63, 52, 53, 54, 55, 56, 57, 58, 59, 60, 61, 255, 255, 50 | 255, 0, 255, 255, 255, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 51 | 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 52 | 25, 255, 255, 255, 255, 255, 255, 26, 27, 28, 29, 30, 31, 32, 33, 53 | 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 48, 54 | 49, 50, 51, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 55 | 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 56 | 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 57 | 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 58 | 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 59 | 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 60 | 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 61 | 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 62 | 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 63 | 255, 64 | }; 65 | 66 | std::vector DecodeBase64(const std::string &input) { 67 | typedef std::vector ret_type; 68 | if (input.empty()) 69 | return ret_type(); 70 | 71 | ret_type ret(3 * input.size() / 4 + 1); 72 | unsigned char *out = &ret[0]; 73 | 74 | unsigned value = 0; 75 | for (std::size_t i = 0; i < input.size(); i++) { 76 | unsigned char d = decoding[static_cast(input[i])]; 77 | if (d == 255) 78 | return ret_type(); 79 | 80 | value = (value << 6) | d; 81 | if (i % 4 == 3) { 82 | *out++ = value >> 16; 83 | if (i > 0 && input[i - 1] != '=') 84 | *out++ = value >> 8; 85 | if (input[i] != '=') 86 | *out++ = value; 87 | } 88 | } 89 | 90 | ret.resize(out - &ret[0]); 91 | return ret; 92 | } 93 | } 94 | -------------------------------------------------------------------------------- /cyberdog_gazebo/third-party/ParamHandler/src/collectionstack.h: -------------------------------------------------------------------------------- 1 | #ifndef COLLECTIONSTACK_H_62B23520_7C8E_11DE_8A39_0800200C9A66 2 | #define COLLECTIONSTACK_H_62B23520_7C8E_11DE_8A39_0800200C9A66 3 | 4 | #if defined(_MSC_VER) || \ 5 | (defined(__GNUC__) && (__GNUC__ == 3 && __GNUC_MINOR__ >= 4) || \ 6 | (__GNUC__ >= 4)) // GCC supports "pragma once" correctly since 3.4 7 | #pragma once 8 | #endif 9 | 10 | #include 11 | #include 12 | 13 | namespace dynacore_YAML { 14 | struct CollectionType { 15 | enum value { NoCollection, BlockMap, BlockSeq, FlowMap, FlowSeq, CompactMap }; 16 | }; 17 | 18 | class CollectionStack { 19 | public: 20 | CollectionType::value GetCurCollectionType() const { 21 | if (collectionStack.empty()) 22 | return CollectionType::NoCollection; 23 | return collectionStack.top(); 24 | } 25 | 26 | void PushCollectionType(CollectionType::value type) { 27 | collectionStack.push(type); 28 | } 29 | void PopCollectionType(CollectionType::value type) { 30 | assert(type == GetCurCollectionType()); 31 | collectionStack.pop(); 32 | } 33 | 34 | private: 35 | std::stack collectionStack; 36 | }; 37 | } 38 | 39 | #endif // COLLECTIONSTACK_H_62B23520_7C8E_11DE_8A39_0800200C9A66 40 | -------------------------------------------------------------------------------- /cyberdog_gazebo/third-party/ParamHandler/src/contrib/graphbuilder.cpp: -------------------------------------------------------------------------------- 1 | #include "graphbuilderadapter.h" 2 | 3 | #include "dynacore_yaml-cpp/parser.h" // IWYU pragma: keep 4 | 5 | namespace dynacore_YAML { 6 | class GraphBuilderInterface; 7 | 8 | void* BuildGraphOfNextDocument(Parser& parser, 9 | GraphBuilderInterface& graphBuilder) { 10 | GraphBuilderAdapter eventHandler(graphBuilder); 11 | if (parser.HandleNextDocument(eventHandler)) { 12 | return eventHandler.RootNode(); 13 | } else { 14 | return NULL; 15 | } 16 | } 17 | } 18 | -------------------------------------------------------------------------------- /cyberdog_gazebo/third-party/ParamHandler/src/contrib/graphbuilderadapter.cpp: -------------------------------------------------------------------------------- 1 | #include "graphbuilderadapter.h" 2 | #include "dynacore_yaml-cpp/contrib/graphbuilder.h" 3 | 4 | namespace dynacore_YAML { 5 | struct Mark; 6 | 7 | int GraphBuilderAdapter::ContainerFrame::sequenceMarker; 8 | 9 | void GraphBuilderAdapter::OnNull(const Mark &mark, anchor_t anchor) { 10 | void *pParent = GetCurrentParent(); 11 | void *pNode = m_builder.NewNull(mark, pParent); 12 | RegisterAnchor(anchor, pNode); 13 | 14 | DispositionNode(pNode); 15 | } 16 | 17 | void GraphBuilderAdapter::OnAlias(const Mark &mark, anchor_t anchor) { 18 | void *pReffedNode = m_anchors.Get(anchor); 19 | DispositionNode(m_builder.AnchorReference(mark, pReffedNode)); 20 | } 21 | 22 | void GraphBuilderAdapter::OnScalar(const Mark &mark, const std::string &tag, 23 | anchor_t anchor, const std::string &value) { 24 | void *pParent = GetCurrentParent(); 25 | void *pNode = m_builder.NewScalar(mark, tag, pParent, value); 26 | RegisterAnchor(anchor, pNode); 27 | 28 | DispositionNode(pNode); 29 | } 30 | 31 | void GraphBuilderAdapter::OnSequenceStart(const Mark &mark, 32 | const std::string &tag, 33 | anchor_t anchor, 34 | EmitterStyle::value /* style */) { 35 | void *pNode = m_builder.NewSequence(mark, tag, GetCurrentParent()); 36 | m_containers.push(ContainerFrame(pNode)); 37 | RegisterAnchor(anchor, pNode); 38 | } 39 | 40 | void GraphBuilderAdapter::OnSequenceEnd() { 41 | void *pSequence = m_containers.top().pContainer; 42 | m_containers.pop(); 43 | 44 | DispositionNode(pSequence); 45 | } 46 | 47 | void GraphBuilderAdapter::OnMapStart(const Mark &mark, const std::string &tag, 48 | anchor_t anchor, 49 | EmitterStyle::value /* style */) { 50 | void *pNode = m_builder.NewMap(mark, tag, GetCurrentParent()); 51 | m_containers.push(ContainerFrame(pNode, m_pKeyNode)); 52 | m_pKeyNode = NULL; 53 | RegisterAnchor(anchor, pNode); 54 | } 55 | 56 | void GraphBuilderAdapter::OnMapEnd() { 57 | void *pMap = m_containers.top().pContainer; 58 | m_pKeyNode = m_containers.top().pPrevKeyNode; 59 | m_containers.pop(); 60 | DispositionNode(pMap); 61 | } 62 | 63 | void *GraphBuilderAdapter::GetCurrentParent() const { 64 | if (m_containers.empty()) { 65 | return NULL; 66 | } 67 | return m_containers.top().pContainer; 68 | } 69 | 70 | void GraphBuilderAdapter::RegisterAnchor(anchor_t anchor, void *pNode) { 71 | if (anchor) { 72 | m_anchors.Register(anchor, pNode); 73 | } 74 | } 75 | 76 | void GraphBuilderAdapter::DispositionNode(void *pNode) { 77 | if (m_containers.empty()) { 78 | m_pRootNode = pNode; 79 | return; 80 | } 81 | 82 | void *pContainer = m_containers.top().pContainer; 83 | if (m_containers.top().isMap()) { 84 | if (m_pKeyNode) { 85 | m_builder.AssignInMap(pContainer, m_pKeyNode, pNode); 86 | m_pKeyNode = NULL; 87 | } else { 88 | m_pKeyNode = pNode; 89 | } 90 | } else { 91 | m_builder.AppendToSequence(pContainer, pNode); 92 | } 93 | } 94 | } 95 | -------------------------------------------------------------------------------- /cyberdog_gazebo/third-party/ParamHandler/src/contrib/graphbuilderadapter.h: -------------------------------------------------------------------------------- 1 | #ifndef GRAPHBUILDERADAPTER_H_62B23520_7C8E_11DE_8A39_0800200C9A66 2 | #define GRAPHBUILDERADAPTER_H_62B23520_7C8E_11DE_8A39_0800200C9A66 3 | 4 | #if defined(_MSC_VER) || \ 5 | (defined(__GNUC__) && (__GNUC__ == 3 && __GNUC_MINOR__ >= 4) || \ 6 | (__GNUC__ >= 4)) // GCC supports "pragma once" correctly since 3.4 7 | #pragma once 8 | #endif 9 | 10 | #include 11 | #include 12 | #include 13 | 14 | #include "dynacore_yaml-cpp/anchor.h" 15 | #include "dynacore_yaml-cpp/contrib/anchordict.h" 16 | #include "dynacore_yaml-cpp/contrib/graphbuilder.h" 17 | #include "dynacore_yaml-cpp/emitterstyle.h" 18 | #include "dynacore_yaml-cpp/eventhandler.h" 19 | 20 | namespace dynacore_YAML { 21 | class GraphBuilderInterface; 22 | struct Mark; 23 | } // namespace YAML 24 | 25 | namespace dynacore_YAML { 26 | class GraphBuilderAdapter : public EventHandler { 27 | public: 28 | GraphBuilderAdapter(GraphBuilderInterface& builder) 29 | : m_builder(builder), m_pRootNode(NULL), m_pKeyNode(NULL) {} 30 | 31 | virtual void OnDocumentStart(const Mark& mark) { (void)mark; } 32 | virtual void OnDocumentEnd() {} 33 | 34 | virtual void OnNull(const Mark& mark, anchor_t anchor); 35 | virtual void OnAlias(const Mark& mark, anchor_t anchor); 36 | virtual void OnScalar(const Mark& mark, const std::string& tag, 37 | anchor_t anchor, const std::string& value); 38 | 39 | virtual void OnSequenceStart(const Mark& mark, const std::string& tag, 40 | anchor_t anchor, EmitterStyle::value style); 41 | virtual void OnSequenceEnd(); 42 | 43 | virtual void OnMapStart(const Mark& mark, const std::string& tag, 44 | anchor_t anchor, EmitterStyle::value style); 45 | virtual void OnMapEnd(); 46 | 47 | void* RootNode() const { return m_pRootNode; } 48 | 49 | private: 50 | struct ContainerFrame { 51 | ContainerFrame(void* pSequence) 52 | : pContainer(pSequence), pPrevKeyNode(&sequenceMarker) {} 53 | ContainerFrame(void* pMap, void* pPrevKeyNode) 54 | : pContainer(pMap), pPrevKeyNode(pPrevKeyNode) {} 55 | 56 | void* pContainer; 57 | void* pPrevKeyNode; 58 | 59 | bool isMap() const { return pPrevKeyNode != &sequenceMarker; } 60 | 61 | private: 62 | static int sequenceMarker; 63 | }; 64 | typedef std::stack ContainerStack; 65 | typedef AnchorDict AnchorMap; 66 | 67 | GraphBuilderInterface& m_builder; 68 | ContainerStack m_containers; 69 | AnchorMap m_anchors; 70 | void* m_pRootNode; 71 | void* m_pKeyNode; 72 | 73 | void* GetCurrentParent() const; 74 | void RegisterAnchor(anchor_t anchor, void* pNode); 75 | void DispositionNode(void* pNode); 76 | }; 77 | } 78 | 79 | #endif // GRAPHBUILDERADAPTER_H_62B23520_7C8E_11DE_8A39_0800200C9A66 80 | -------------------------------------------------------------------------------- /cyberdog_gazebo/third-party/ParamHandler/src/convert.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "dynacore_yaml-cpp/node/convert.h" 4 | 5 | namespace { 6 | // we're not gonna mess with the mess that is all the isupper/etc. functions 7 | bool IsLower(char ch) { return 'a' <= ch && ch <= 'z'; } 8 | bool IsUpper(char ch) { return 'A' <= ch && ch <= 'Z'; } 9 | char ToLower(char ch) { return IsUpper(ch) ? ch + 'a' - 'A' : ch; } 10 | 11 | std::string tolower(const std::string& str) { 12 | std::string s(str); 13 | std::transform(s.begin(), s.end(), s.begin(), ToLower); 14 | return s; 15 | } 16 | 17 | template 18 | bool IsEntirely(const std::string& str, T func) { 19 | for (std::size_t i = 0; i < str.size(); i++) 20 | if (!func(str[i])) 21 | return false; 22 | 23 | return true; 24 | } 25 | 26 | // IsFlexibleCase 27 | // . Returns true if 'str' is: 28 | // . UPPERCASE 29 | // . lowercase 30 | // . Capitalized 31 | bool IsFlexibleCase(const std::string& str) { 32 | if (str.empty()) 33 | return true; 34 | 35 | if (IsEntirely(str, IsLower)) 36 | return true; 37 | 38 | bool firstcaps = IsUpper(str[0]); 39 | std::string rest = str.substr(1); 40 | return firstcaps && (IsEntirely(rest, IsLower) || IsEntirely(rest, IsUpper)); 41 | } 42 | } 43 | 44 | namespace dynacore_YAML { 45 | bool convert::decode(const Node& node, bool& rhs) { 46 | if (!node.IsScalar()) 47 | return false; 48 | 49 | // we can't use iostream bool extraction operators as they don't 50 | // recognize all possible values in the table below (taken from 51 | // http://yaml.org/type/bool.html) 52 | static const struct { 53 | std::string truename, falsename; 54 | } names[] = { 55 | {"y", "n"}, {"yes", "no"}, {"true", "false"}, {"on", "off"}, 56 | }; 57 | 58 | if (!IsFlexibleCase(node.Scalar())) 59 | return false; 60 | 61 | for (unsigned i = 0; i < sizeof(names) / sizeof(names[0]); i++) { 62 | if (names[i].truename == tolower(node.Scalar())) { 63 | rhs = true; 64 | return true; 65 | } 66 | 67 | if (names[i].falsename == tolower(node.Scalar())) { 68 | rhs = false; 69 | return true; 70 | } 71 | } 72 | 73 | return false; 74 | } 75 | } 76 | -------------------------------------------------------------------------------- /cyberdog_gazebo/third-party/ParamHandler/src/directives.cpp: -------------------------------------------------------------------------------- 1 | #include "directives.h" 2 | 3 | namespace dynacore_YAML { 4 | Directives::Directives() { 5 | // version 6 | version.isDefault = true; 7 | version.major = 1; 8 | version.minor = 2; 9 | } 10 | 11 | const std::string Directives::TranslateTagHandle( 12 | const std::string& handle) const { 13 | std::map::const_iterator it = tags.find(handle); 14 | if (it == tags.end()) { 15 | if (handle == "!!") 16 | return "tag:yaml.org,2002:"; 17 | return handle; 18 | } 19 | 20 | return it->second; 21 | } 22 | } 23 | -------------------------------------------------------------------------------- /cyberdog_gazebo/third-party/ParamHandler/src/directives.h: -------------------------------------------------------------------------------- 1 | #ifndef DIRECTIVES_H_62B23520_7C8E_11DE_8A39_0800200C9A66 2 | #define DIRECTIVES_H_62B23520_7C8E_11DE_8A39_0800200C9A66 3 | 4 | #if defined(_MSC_VER) || \ 5 | (defined(__GNUC__) && (__GNUC__ == 3 && __GNUC_MINOR__ >= 4) || \ 6 | (__GNUC__ >= 4)) // GCC supports "pragma once" correctly since 3.4 7 | #pragma once 8 | #endif 9 | 10 | #include 11 | #include 12 | 13 | namespace dynacore_YAML { 14 | struct Version { 15 | bool isDefault; 16 | int major, minor; 17 | }; 18 | 19 | struct Directives { 20 | Directives(); 21 | 22 | const std::string TranslateTagHandle(const std::string& handle) const; 23 | 24 | Version version; 25 | std::map tags; 26 | }; 27 | } 28 | 29 | #endif // DIRECTIVES_H_62B23520_7C8E_11DE_8A39_0800200C9A66 30 | -------------------------------------------------------------------------------- /cyberdog_gazebo/third-party/ParamHandler/src/emit.cpp: -------------------------------------------------------------------------------- 1 | #include "dynacore_yaml-cpp/node/emit.h" 2 | #include "dynacore_yaml-cpp/emitfromevents.h" 3 | #include "dynacore_yaml-cpp/emitter.h" 4 | #include "nodeevents.h" 5 | 6 | namespace dynacore_YAML { 7 | Emitter& operator<<(Emitter& out, const Node& node) { 8 | EmitFromEvents emitFromEvents(out); 9 | NodeEvents events(node); 10 | events.Emit(emitFromEvents); 11 | return out; 12 | } 13 | 14 | std::ostream& operator<<(std::ostream& out, const Node& node) { 15 | Emitter emitter(out); 16 | emitter << node; 17 | return out; 18 | } 19 | 20 | std::string Dump(const Node& node) { 21 | Emitter emitter; 22 | emitter << node; 23 | return emitter.c_str(); 24 | } 25 | } // namespace YAML 26 | -------------------------------------------------------------------------------- /cyberdog_gazebo/third-party/ParamHandler/src/emitfromevents.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include "dynacore_yaml-cpp/emitfromevents.h" 5 | #include "dynacore_yaml-cpp/emitter.h" 6 | #include "dynacore_yaml-cpp/emittermanip.h" 7 | #include "dynacore_yaml-cpp/null.h" 8 | 9 | namespace dynacore_YAML { 10 | struct Mark; 11 | } // namespace YAML 12 | 13 | namespace { 14 | std::string ToString(dynacore_YAML::anchor_t anchor) { 15 | std::stringstream stream; 16 | stream << anchor; 17 | return stream.str(); 18 | } 19 | } 20 | 21 | namespace dynacore_YAML { 22 | EmitFromEvents::EmitFromEvents(Emitter& emitter) : m_emitter(emitter) {} 23 | 24 | void EmitFromEvents::OnDocumentStart(const Mark&) {} 25 | 26 | void EmitFromEvents::OnDocumentEnd() {} 27 | 28 | void EmitFromEvents::OnNull(const Mark&, anchor_t anchor) { 29 | BeginNode(); 30 | EmitProps("", anchor); 31 | m_emitter << Null; 32 | } 33 | 34 | void EmitFromEvents::OnAlias(const Mark&, anchor_t anchor) { 35 | BeginNode(); 36 | m_emitter << Alias(ToString(anchor)); 37 | } 38 | 39 | void EmitFromEvents::OnScalar(const Mark&, const std::string& tag, 40 | anchor_t anchor, const std::string& value) { 41 | BeginNode(); 42 | EmitProps(tag, anchor); 43 | m_emitter << value; 44 | } 45 | 46 | void EmitFromEvents::OnSequenceStart(const Mark&, const std::string& tag, 47 | anchor_t anchor, 48 | EmitterStyle::value style) { 49 | BeginNode(); 50 | EmitProps(tag, anchor); 51 | switch (style) { 52 | case EmitterStyle::Block: 53 | m_emitter << Block; 54 | break; 55 | case EmitterStyle::Flow: 56 | m_emitter << Flow; 57 | break; 58 | default: 59 | break; 60 | } 61 | m_emitter << BeginSeq; 62 | m_stateStack.push(State::WaitingForSequenceEntry); 63 | } 64 | 65 | void EmitFromEvents::OnSequenceEnd() { 66 | m_emitter << EndSeq; 67 | assert(m_stateStack.top() == State::WaitingForSequenceEntry); 68 | m_stateStack.pop(); 69 | } 70 | 71 | void EmitFromEvents::OnMapStart(const Mark&, const std::string& tag, 72 | anchor_t anchor, EmitterStyle::value style) { 73 | BeginNode(); 74 | EmitProps(tag, anchor); 75 | switch (style) { 76 | case EmitterStyle::Block: 77 | m_emitter << Block; 78 | break; 79 | case EmitterStyle::Flow: 80 | m_emitter << Flow; 81 | break; 82 | default: 83 | break; 84 | } 85 | m_emitter << BeginMap; 86 | m_stateStack.push(State::WaitingForKey); 87 | } 88 | 89 | void EmitFromEvents::OnMapEnd() { 90 | m_emitter << EndMap; 91 | assert(m_stateStack.top() == State::WaitingForKey); 92 | m_stateStack.pop(); 93 | } 94 | 95 | void EmitFromEvents::BeginNode() { 96 | if (m_stateStack.empty()) 97 | return; 98 | 99 | switch (m_stateStack.top()) { 100 | case State::WaitingForKey: 101 | m_emitter << Key; 102 | m_stateStack.top() = State::WaitingForValue; 103 | break; 104 | case State::WaitingForValue: 105 | m_emitter << Value; 106 | m_stateStack.top() = State::WaitingForKey; 107 | break; 108 | default: 109 | break; 110 | } 111 | } 112 | 113 | void EmitFromEvents::EmitProps(const std::string& tag, anchor_t anchor) { 114 | if (!tag.empty() && tag != "?" && tag != "!") 115 | m_emitter << VerbatimTag(tag); 116 | if (anchor) 117 | m_emitter << Anchor(ToString(anchor)); 118 | } 119 | } 120 | -------------------------------------------------------------------------------- /cyberdog_gazebo/third-party/ParamHandler/src/emitterutils.h: -------------------------------------------------------------------------------- 1 | #ifndef EMITTERUTILS_H_62B23520_7C8E_11DE_8A39_0800200C9A66 2 | #define EMITTERUTILS_H_62B23520_7C8E_11DE_8A39_0800200C9A66 3 | 4 | #if defined(_MSC_VER) || \ 5 | (defined(__GNUC__) && (__GNUC__ == 3 && __GNUC_MINOR__ >= 4) || \ 6 | (__GNUC__ >= 4)) // GCC supports "pragma once" correctly since 3.4 7 | #pragma once 8 | #endif 9 | 10 | #include 11 | 12 | #include "emitterstate.h" 13 | #include "dynacore_yaml-cpp/emittermanip.h" 14 | #include "dynacore_yaml-cpp/ostream_wrapper.h" 15 | 16 | namespace dynacore_YAML { 17 | class ostream_wrapper; 18 | } // namespace YAML 19 | 20 | namespace dynacore_YAML { 21 | class Binary; 22 | 23 | struct StringFormat { 24 | enum value { Plain, SingleQuoted, DoubleQuoted, Literal }; 25 | }; 26 | 27 | namespace Utils { 28 | StringFormat::value ComputeStringFormat(const std::string& str, 29 | EMITTER_MANIP strFormat, 30 | FlowType::value flowType, 31 | bool escapeNonAscii); 32 | 33 | bool WriteSingleQuotedString(ostream_wrapper& out, const std::string& str); 34 | bool WriteDoubleQuotedString(ostream_wrapper& out, const std::string& str, 35 | bool escapeNonAscii); 36 | bool WriteLiteralString(ostream_wrapper& out, const std::string& str, 37 | std::size_t indent); 38 | bool WriteChar(ostream_wrapper& out, char ch); 39 | bool WriteComment(ostream_wrapper& out, const std::string& str, 40 | std::size_t postCommentIndent); 41 | bool WriteAlias(ostream_wrapper& out, const std::string& str); 42 | bool WriteAnchor(ostream_wrapper& out, const std::string& str); 43 | bool WriteTag(ostream_wrapper& out, const std::string& str, bool verbatim); 44 | bool WriteTagWithPrefix(ostream_wrapper& out, const std::string& prefix, 45 | const std::string& tag); 46 | bool WriteBinary(ostream_wrapper& out, const Binary& binary); 47 | } 48 | } 49 | 50 | #endif // EMITTERUTILS_H_62B23520_7C8E_11DE_8A39_0800200C9A66 51 | -------------------------------------------------------------------------------- /cyberdog_gazebo/third-party/ParamHandler/src/exceptions.cpp: -------------------------------------------------------------------------------- 1 | #include "dynacore_yaml-cpp/exceptions.h" 2 | 3 | // This is here for compatibility with older versions of Visual Studio 4 | // which don't support noexcept 5 | #ifdef _MSC_VER 6 | #define YAML_CPP_NOEXCEPT _NOEXCEPT 7 | #else 8 | #define YAML_CPP_NOEXCEPT noexcept 9 | #endif 10 | 11 | namespace dynacore_YAML { 12 | 13 | // These destructors are defined out-of-line so the vtable is only emitted once. 14 | Exception::~Exception() YAML_CPP_NOEXCEPT {} 15 | ParserException::~ParserException() YAML_CPP_NOEXCEPT {} 16 | RepresentationException::~RepresentationException() YAML_CPP_NOEXCEPT {} 17 | InvalidScalar::~InvalidScalar() YAML_CPP_NOEXCEPT {} 18 | KeyNotFound::~KeyNotFound() YAML_CPP_NOEXCEPT {} 19 | InvalidNode::~InvalidNode() YAML_CPP_NOEXCEPT {} 20 | BadConversion::~BadConversion() YAML_CPP_NOEXCEPT {} 21 | BadDereference::~BadDereference() YAML_CPP_NOEXCEPT {} 22 | BadSubscript::~BadSubscript() YAML_CPP_NOEXCEPT {} 23 | BadPushback::~BadPushback() YAML_CPP_NOEXCEPT {} 24 | BadInsert::~BadInsert() YAML_CPP_NOEXCEPT {} 25 | EmitterException::~EmitterException() YAML_CPP_NOEXCEPT {} 26 | BadFile::~BadFile() YAML_CPP_NOEXCEPT {} 27 | } 28 | 29 | #undef YAML_CPP_NOEXCEPT 30 | 31 | 32 | -------------------------------------------------------------------------------- /cyberdog_gazebo/third-party/ParamHandler/src/exp.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "exp.h" 4 | #include "stream.h" 5 | #include "dynacore_yaml-cpp/exceptions.h" // IWYU pragma: keep 6 | 7 | namespace dynacore_YAML{ 8 | struct Mark; 9 | } // namespace YAML 10 | 11 | namespace dynacore_YAML { 12 | namespace Exp { 13 | unsigned ParseHex(const std::string& str, const Mark& mark) { 14 | unsigned value = 0; 15 | for (std::size_t i = 0; i < str.size(); i++) { 16 | char ch = str[i]; 17 | int digit = 0; 18 | if ('a' <= ch && ch <= 'f') 19 | digit = ch - 'a' + 10; 20 | else if ('A' <= ch && ch <= 'F') 21 | digit = ch - 'A' + 10; 22 | else if ('0' <= ch && ch <= '9') 23 | digit = ch - '0'; 24 | else 25 | throw ParserException(mark, ErrorMsg::INVALID_HEX); 26 | 27 | value = (value << 4) + digit; 28 | } 29 | 30 | return value; 31 | } 32 | 33 | std::string Str(unsigned ch) { return std::string(1, static_cast(ch)); } 34 | 35 | // Escape 36 | // . Translates the next 'codeLength' characters into a hex number and returns 37 | // the result. 38 | // . Throws if it's not actually hex. 39 | std::string Escape(Stream& in, int codeLength) { 40 | // grab string 41 | std::string str; 42 | for (int i = 0; i < codeLength; i++) 43 | str += in.get(); 44 | 45 | // get the value 46 | unsigned value = ParseHex(str, in.mark()); 47 | 48 | // legal unicode? 49 | if ((value >= 0xD800 && value <= 0xDFFF) || value > 0x10FFFF) { 50 | std::stringstream msg; 51 | msg << ErrorMsg::INVALID_UNICODE << value; 52 | throw ParserException(in.mark(), msg.str()); 53 | } 54 | 55 | // now break it up into chars 56 | if (value <= 0x7F) 57 | return Str(value); 58 | else if (value <= 0x7FF) 59 | return Str(0xC0 + (value >> 6)) + Str(0x80 + (value & 0x3F)); 60 | else if (value <= 0xFFFF) 61 | return Str(0xE0 + (value >> 12)) + Str(0x80 + ((value >> 6) & 0x3F)) + 62 | Str(0x80 + (value & 0x3F)); 63 | else 64 | return Str(0xF0 + (value >> 18)) + Str(0x80 + ((value >> 12) & 0x3F)) + 65 | Str(0x80 + ((value >> 6) & 0x3F)) + Str(0x80 + (value & 0x3F)); 66 | } 67 | 68 | // Escape 69 | // . Escapes the sequence starting 'in' (it must begin with a '\' or single 70 | // quote) 71 | // and returns the result. 72 | // . Throws if it's an unknown escape character. 73 | std::string Escape(Stream& in) { 74 | // eat slash 75 | char escape = in.get(); 76 | 77 | // switch on escape character 78 | char ch = in.get(); 79 | 80 | // first do single quote, since it's easier 81 | if (escape == '\'' && ch == '\'') 82 | return "\'"; 83 | 84 | // now do the slash (we're not gonna check if it's a slash - you better pass 85 | // one!) 86 | switch (ch) { 87 | case '0': 88 | return std::string(1, '\x00'); 89 | case 'a': 90 | return "\x07"; 91 | case 'b': 92 | return "\x08"; 93 | case 't': 94 | case '\t': 95 | return "\x09"; 96 | case 'n': 97 | return "\x0A"; 98 | case 'v': 99 | return "\x0B"; 100 | case 'f': 101 | return "\x0C"; 102 | case 'r': 103 | return "\x0D"; 104 | case 'e': 105 | return "\x1B"; 106 | case ' ': 107 | return "\x20"; 108 | case '\"': 109 | return "\""; 110 | case '\'': 111 | return "\'"; 112 | case '\\': 113 | return "\\"; 114 | case '/': 115 | return "/"; 116 | case 'N': 117 | return "\x85"; 118 | case '_': 119 | return "\xA0"; 120 | case 'L': 121 | return "\xE2\x80\xA8"; // LS (#x2028) 122 | case 'P': 123 | return "\xE2\x80\xA9"; // PS (#x2029) 124 | case 'x': 125 | return Escape(in, 2); 126 | case 'u': 127 | return Escape(in, 4); 128 | case 'U': 129 | return Escape(in, 8); 130 | } 131 | 132 | std::stringstream msg; 133 | throw ParserException(in.mark(), std::string(ErrorMsg::INVALID_ESCAPE) + ch); 134 | } 135 | } 136 | } 137 | -------------------------------------------------------------------------------- /cyberdog_gazebo/third-party/ParamHandler/src/indentation.h: -------------------------------------------------------------------------------- 1 | #ifndef INDENTATION_H_62B23520_7C8E_11DE_8A39_0800200C9A66 2 | #define INDENTATION_H_62B23520_7C8E_11DE_8A39_0800200C9A66 3 | 4 | #if defined(_MSC_VER) || \ 5 | (defined(__GNUC__) && (__GNUC__ == 3 && __GNUC_MINOR__ >= 4) || \ 6 | (__GNUC__ >= 4)) // GCC supports "pragma once" correctly since 3.4 7 | #pragma once 8 | #endif 9 | 10 | #include 11 | #include 12 | 13 | #include "dynacore_yaml-cpp/ostream_wrapper.h" 14 | 15 | namespace dynacore_YAML { 16 | struct Indentation { 17 | Indentation(std::size_t n_) : n(n_) {} 18 | std::size_t n; 19 | }; 20 | 21 | inline ostream_wrapper& operator<<(ostream_wrapper& out, 22 | const Indentation& indent) { 23 | for (std::size_t i = 0; i < indent.n; i++) 24 | out << ' '; 25 | return out; 26 | } 27 | 28 | struct IndentTo { 29 | IndentTo(std::size_t n_) : n(n_) {} 30 | std::size_t n; 31 | }; 32 | 33 | inline ostream_wrapper& operator<<(ostream_wrapper& out, 34 | const IndentTo& indent) { 35 | while (out.col() < indent.n) 36 | out << ' '; 37 | return out; 38 | } 39 | } 40 | 41 | #endif // INDENTATION_H_62B23520_7C8E_11DE_8A39_0800200C9A66 42 | -------------------------------------------------------------------------------- /cyberdog_gazebo/third-party/ParamHandler/src/memory.cpp: -------------------------------------------------------------------------------- 1 | #include "dynacore_yaml-cpp/node/detail/memory.h" 2 | #include "dynacore_yaml-cpp/node/detail/node.h" // IWYU pragma: keep 3 | #include "dynacore_yaml-cpp/node/ptr.h" 4 | 5 | namespace dynacore_YAML { 6 | namespace detail { 7 | 8 | void memory_holder::merge(memory_holder& rhs) { 9 | if (m_pMemory == rhs.m_pMemory) 10 | return; 11 | 12 | m_pMemory->merge(*rhs.m_pMemory); 13 | rhs.m_pMemory = m_pMemory; 14 | } 15 | 16 | node& memory::create_node() { 17 | shared_node pNode(new node); 18 | m_nodes.insert(pNode); 19 | return *pNode; 20 | } 21 | 22 | void memory::merge(const memory& rhs) { 23 | m_nodes.insert(rhs.m_nodes.begin(), rhs.m_nodes.end()); 24 | } 25 | } 26 | } 27 | -------------------------------------------------------------------------------- /cyberdog_gazebo/third-party/ParamHandler/src/node.cpp: -------------------------------------------------------------------------------- 1 | #include "dynacore_yaml-cpp/node/node.h" 2 | #include "nodebuilder.h" 3 | #include "nodeevents.h" 4 | 5 | namespace dynacore_YAML { 6 | Node Clone(const Node& node) { 7 | NodeEvents events(node); 8 | NodeBuilder builder; 9 | events.Emit(builder); 10 | return builder.Root(); 11 | } 12 | } 13 | -------------------------------------------------------------------------------- /cyberdog_gazebo/third-party/ParamHandler/src/nodebuilder.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include "nodebuilder.h" 5 | #include "dynacore_yaml-cpp/node/detail/node.h" 6 | #include "dynacore_yaml-cpp/node/impl.h" 7 | #include "dynacore_yaml-cpp/node/node.h" 8 | #include "dynacore_yaml-cpp/node/type.h" 9 | 10 | namespace dynacore_YAML { 11 | struct Mark; 12 | 13 | NodeBuilder::NodeBuilder() 14 | : m_pMemory(new detail::memory_holder), m_pRoot(0), m_mapDepth(0) { 15 | m_anchors.push_back(0); // since the anchors start at 1 16 | } 17 | 18 | NodeBuilder::~NodeBuilder() {} 19 | 20 | Node NodeBuilder::Root() { 21 | if (!m_pRoot) 22 | return Node(); 23 | 24 | return Node(*m_pRoot, m_pMemory); 25 | } 26 | 27 | void NodeBuilder::OnDocumentStart(const Mark&) {} 28 | 29 | void NodeBuilder::OnDocumentEnd() {} 30 | 31 | void NodeBuilder::OnNull(const Mark& mark, anchor_t anchor) { 32 | detail::node& node = Push(mark, anchor); 33 | node.set_null(); 34 | Pop(); 35 | } 36 | 37 | void NodeBuilder::OnAlias(const Mark& /* mark */, anchor_t anchor) { 38 | detail::node& node = *m_anchors[anchor]; 39 | Push(node); 40 | Pop(); 41 | } 42 | 43 | void NodeBuilder::OnScalar(const Mark& mark, const std::string& tag, 44 | anchor_t anchor, const std::string& value) { 45 | detail::node& node = Push(mark, anchor); 46 | node.set_scalar(value); 47 | node.set_tag(tag); 48 | Pop(); 49 | } 50 | 51 | void NodeBuilder::OnSequenceStart(const Mark& mark, const std::string& tag, 52 | anchor_t anchor, EmitterStyle::value style) { 53 | detail::node& node = Push(mark, anchor); 54 | node.set_tag(tag); 55 | node.set_type(NodeType::Sequence); 56 | node.set_style(style); 57 | } 58 | 59 | void NodeBuilder::OnSequenceEnd() { Pop(); } 60 | 61 | void NodeBuilder::OnMapStart(const Mark& mark, const std::string& tag, 62 | anchor_t anchor, EmitterStyle::value style) { 63 | detail::node& node = Push(mark, anchor); 64 | node.set_type(NodeType::Map); 65 | node.set_tag(tag); 66 | node.set_style(style); 67 | m_mapDepth++; 68 | } 69 | 70 | void NodeBuilder::OnMapEnd() { 71 | assert(m_mapDepth > 0); 72 | m_mapDepth--; 73 | Pop(); 74 | } 75 | 76 | detail::node& NodeBuilder::Push(const Mark& mark, anchor_t anchor) { 77 | detail::node& node = m_pMemory->create_node(); 78 | node.set_mark(mark); 79 | RegisterAnchor(anchor, node); 80 | Push(node); 81 | return node; 82 | } 83 | 84 | void NodeBuilder::Push(detail::node& node) { 85 | const bool needsKey = 86 | (!m_stack.empty() && m_stack.back()->type() == NodeType::Map && 87 | m_keys.size() < m_mapDepth); 88 | 89 | m_stack.push_back(&node); 90 | if (needsKey) 91 | m_keys.push_back(PushedKey(&node, false)); 92 | } 93 | 94 | void NodeBuilder::Pop() { 95 | assert(!m_stack.empty()); 96 | if (m_stack.size() == 1) { 97 | m_pRoot = m_stack[0]; 98 | m_stack.pop_back(); 99 | return; 100 | } 101 | 102 | detail::node& node = *m_stack.back(); 103 | m_stack.pop_back(); 104 | 105 | detail::node& collection = *m_stack.back(); 106 | 107 | if (collection.type() == NodeType::Sequence) { 108 | collection.push_back(node, m_pMemory); 109 | } else if (collection.type() == NodeType::Map) { 110 | assert(!m_keys.empty()); 111 | PushedKey& key = m_keys.back(); 112 | if (key.second) { 113 | collection.insert(*key.first, node, m_pMemory); 114 | m_keys.pop_back(); 115 | } else { 116 | key.second = true; 117 | } 118 | } else { 119 | assert(false); 120 | m_stack.clear(); 121 | } 122 | } 123 | 124 | void NodeBuilder::RegisterAnchor(anchor_t anchor, detail::node& node) { 125 | if (anchor) { 126 | assert(anchor == m_anchors.size()); 127 | m_anchors.push_back(&node); 128 | } 129 | } 130 | } 131 | -------------------------------------------------------------------------------- /cyberdog_gazebo/third-party/ParamHandler/src/nodebuilder.h: -------------------------------------------------------------------------------- 1 | #ifndef NODE_NODEBUILDER_H_62B23520_7C8E_11DE_8A39_0800200C9A66 2 | #define NODE_NODEBUILDER_H_62B23520_7C8E_11DE_8A39_0800200C9A66 3 | 4 | #if defined(_MSC_VER) || \ 5 | (defined(__GNUC__) && (__GNUC__ == 3 && __GNUC_MINOR__ >= 4) || \ 6 | (__GNUC__ >= 4)) // GCC supports "pragma once" correctly since 3.4 7 | #pragma once 8 | #endif 9 | 10 | #include 11 | 12 | #include "dynacore_yaml-cpp/anchor.h" 13 | #include "dynacore_yaml-cpp/emitterstyle.h" 14 | #include "dynacore_yaml-cpp/eventhandler.h" 15 | #include "dynacore_yaml-cpp/node/ptr.h" 16 | 17 | namespace dynacore_YAML { 18 | namespace detail { 19 | class node; 20 | } // namespace detail 21 | struct Mark; 22 | } // namespace YAML 23 | 24 | namespace dynacore_YAML { 25 | class Node; 26 | 27 | class NodeBuilder : public EventHandler { 28 | public: 29 | NodeBuilder(); 30 | virtual ~NodeBuilder(); 31 | 32 | Node Root(); 33 | 34 | virtual void OnDocumentStart(const Mark& mark); 35 | virtual void OnDocumentEnd(); 36 | 37 | virtual void OnNull(const Mark& mark, anchor_t anchor); 38 | virtual void OnAlias(const Mark& mark, anchor_t anchor); 39 | virtual void OnScalar(const Mark& mark, const std::string& tag, 40 | anchor_t anchor, const std::string& value); 41 | 42 | virtual void OnSequenceStart(const Mark& mark, const std::string& tag, 43 | anchor_t anchor, EmitterStyle::value style); 44 | virtual void OnSequenceEnd(); 45 | 46 | virtual void OnMapStart(const Mark& mark, const std::string& tag, 47 | anchor_t anchor, EmitterStyle::value style); 48 | virtual void OnMapEnd(); 49 | 50 | private: 51 | detail::node& Push(const Mark& mark, anchor_t anchor); 52 | void Push(detail::node& node); 53 | void Pop(); 54 | void RegisterAnchor(anchor_t anchor, detail::node& node); 55 | 56 | private: 57 | detail::shared_memory_holder m_pMemory; 58 | detail::node* m_pRoot; 59 | 60 | typedef std::vector Nodes; 61 | Nodes m_stack; 62 | Nodes m_anchors; 63 | 64 | typedef std::pair PushedKey; 65 | std::vector m_keys; 66 | std::size_t m_mapDepth; 67 | }; 68 | } 69 | 70 | #endif // NODE_NODEBUILDER_H_62B23520_7C8E_11DE_8A39_0800200C9A66 71 | -------------------------------------------------------------------------------- /cyberdog_gazebo/third-party/ParamHandler/src/nodeevents.cpp: -------------------------------------------------------------------------------- 1 | #include "nodeevents.h" 2 | #include "dynacore_yaml-cpp/eventhandler.h" 3 | #include "dynacore_yaml-cpp/mark.h" 4 | #include "dynacore_yaml-cpp/node/detail/node.h" 5 | #include "dynacore_yaml-cpp/node/detail/node_iterator.h" 6 | #include "dynacore_yaml-cpp/node/node.h" 7 | #include "dynacore_yaml-cpp/node/type.h" 8 | 9 | namespace dynacore_YAML { 10 | void NodeEvents::AliasManager::RegisterReference(const detail::node& node) { 11 | m_anchorByIdentity.insert(std::make_pair(node.ref(), _CreateNewAnchor())); 12 | } 13 | 14 | anchor_t NodeEvents::AliasManager::LookupAnchor( 15 | const detail::node& node) const { 16 | AnchorByIdentity::const_iterator it = m_anchorByIdentity.find(node.ref()); 17 | if (it == m_anchorByIdentity.end()) 18 | return 0; 19 | return it->second; 20 | } 21 | 22 | NodeEvents::NodeEvents(const Node& node) 23 | : m_pMemory(node.m_pMemory), m_root(node.m_pNode) { 24 | if (m_root) 25 | Setup(*m_root); 26 | } 27 | 28 | void NodeEvents::Setup(const detail::node& node) { 29 | int& refCount = m_refCount[node.ref()]; 30 | refCount++; 31 | if (refCount > 1) 32 | return; 33 | 34 | if (node.type() == NodeType::Sequence) { 35 | for (detail::const_node_iterator it = node.begin(); it != node.end(); ++it) 36 | Setup(**it); 37 | } else if (node.type() == NodeType::Map) { 38 | for (detail::const_node_iterator it = node.begin(); it != node.end(); 39 | ++it) { 40 | Setup(*it->first); 41 | Setup(*it->second); 42 | } 43 | } 44 | } 45 | 46 | void NodeEvents::Emit(EventHandler& handler) { 47 | AliasManager am; 48 | 49 | handler.OnDocumentStart(Mark()); 50 | if (m_root) 51 | Emit(*m_root, handler, am); 52 | handler.OnDocumentEnd(); 53 | } 54 | 55 | void NodeEvents::Emit(const detail::node& node, EventHandler& handler, 56 | AliasManager& am) const { 57 | anchor_t anchor = NullAnchor; 58 | if (IsAliased(node)) { 59 | anchor = am.LookupAnchor(node); 60 | if (anchor) { 61 | handler.OnAlias(Mark(), anchor); 62 | return; 63 | } 64 | 65 | am.RegisterReference(node); 66 | anchor = am.LookupAnchor(node); 67 | } 68 | 69 | switch (node.type()) { 70 | case NodeType::Undefined: 71 | break; 72 | case NodeType::Null: 73 | handler.OnNull(Mark(), anchor); 74 | break; 75 | case NodeType::Scalar: 76 | handler.OnScalar(Mark(), node.tag(), anchor, node.scalar()); 77 | break; 78 | case NodeType::Sequence: 79 | handler.OnSequenceStart(Mark(), node.tag(), anchor, node.style()); 80 | for (detail::const_node_iterator it = node.begin(); it != node.end(); 81 | ++it) 82 | Emit(**it, handler, am); 83 | handler.OnSequenceEnd(); 84 | break; 85 | case NodeType::Map: 86 | handler.OnMapStart(Mark(), node.tag(), anchor, node.style()); 87 | for (detail::const_node_iterator it = node.begin(); it != node.end(); 88 | ++it) { 89 | Emit(*it->first, handler, am); 90 | Emit(*it->second, handler, am); 91 | } 92 | handler.OnMapEnd(); 93 | break; 94 | } 95 | } 96 | 97 | bool NodeEvents::IsAliased(const detail::node& node) const { 98 | RefCount::const_iterator it = m_refCount.find(node.ref()); 99 | return it != m_refCount.end() && it->second > 1; 100 | } 101 | } 102 | -------------------------------------------------------------------------------- /cyberdog_gazebo/third-party/ParamHandler/src/nodeevents.h: -------------------------------------------------------------------------------- 1 | #ifndef NODE_NODEEVENTS_H_62B23520_7C8E_11DE_8A39_0800200C9A66 2 | #define NODE_NODEEVENTS_H_62B23520_7C8E_11DE_8A39_0800200C9A66 3 | 4 | #if defined(_MSC_VER) || \ 5 | (defined(__GNUC__) && (__GNUC__ == 3 && __GNUC_MINOR__ >= 4) || \ 6 | (__GNUC__ >= 4)) // GCC supports "pragma once" correctly since 3.4 7 | #pragma once 8 | #endif 9 | 10 | #include 11 | #include 12 | 13 | #include "dynacore_yaml-cpp/anchor.h" 14 | #include "dynacore_yaml-cpp/node/ptr.h" 15 | 16 | namespace dynacore_YAML { 17 | namespace detail { 18 | class node; 19 | } // namespace detail 20 | } // namespace YAML 21 | 22 | namespace dynacore_YAML { 23 | class EventHandler; 24 | class Node; 25 | 26 | class NodeEvents { 27 | public: 28 | explicit NodeEvents(const Node& node); 29 | 30 | void Emit(EventHandler& handler); 31 | 32 | private: 33 | class AliasManager { 34 | public: 35 | AliasManager() : m_curAnchor(0) {} 36 | 37 | void RegisterReference(const detail::node& node); 38 | anchor_t LookupAnchor(const detail::node& node) const; 39 | 40 | private: 41 | anchor_t _CreateNewAnchor() { return ++m_curAnchor; } 42 | 43 | private: 44 | typedef std::map AnchorByIdentity; 45 | AnchorByIdentity m_anchorByIdentity; 46 | 47 | anchor_t m_curAnchor; 48 | }; 49 | 50 | void Setup(const detail::node& node); 51 | void Emit(const detail::node& node, EventHandler& handler, 52 | AliasManager& am) const; 53 | bool IsAliased(const detail::node& node) const; 54 | 55 | private: 56 | detail::shared_memory_holder m_pMemory; 57 | detail::node* m_root; 58 | 59 | typedef std::map RefCount; 60 | RefCount m_refCount; 61 | }; 62 | } 63 | 64 | #endif // NODE_NODEEVENTS_H_62B23520_7C8E_11DE_8A39_0800200C9A66 65 | -------------------------------------------------------------------------------- /cyberdog_gazebo/third-party/ParamHandler/src/null.cpp: -------------------------------------------------------------------------------- 1 | #include "dynacore_yaml-cpp/null.h" 2 | 3 | namespace dynacore_YAML { 4 | _Null Null; 5 | 6 | bool IsNullString(const std::string& str) { 7 | return str.empty() || str == "~" || str == "null" || str == "Null" || 8 | str == "NULL"; 9 | } 10 | } 11 | -------------------------------------------------------------------------------- /cyberdog_gazebo/third-party/ParamHandler/src/ostream_wrapper.cpp: -------------------------------------------------------------------------------- 1 | #include "dynacore_yaml-cpp/ostream_wrapper.h" 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | namespace dynacore_YAML { 8 | ostream_wrapper::ostream_wrapper() 9 | : m_buffer(1, '\0'), 10 | m_pStream(0), 11 | m_pos(0), 12 | m_row(0), 13 | m_col(0), 14 | m_comment(false) {} 15 | 16 | ostream_wrapper::ostream_wrapper(std::ostream& stream) 17 | : m_pStream(&stream), m_pos(0), m_row(0), m_col(0), m_comment(false) {} 18 | 19 | ostream_wrapper::~ostream_wrapper() {} 20 | 21 | void ostream_wrapper::write(const std::string& str) { 22 | if (m_pStream) { 23 | m_pStream->write(str.c_str(), str.size()); 24 | } else { 25 | m_buffer.resize(std::max(m_buffer.size(), m_pos + str.size() + 1)); 26 | std::copy(str.begin(), str.end(), m_buffer.begin() + m_pos); 27 | } 28 | 29 | for (std::size_t i = 0; i < str.size(); i++) { 30 | update_pos(str[i]); 31 | } 32 | } 33 | 34 | void ostream_wrapper::write(const char* str, std::size_t size) { 35 | if (m_pStream) { 36 | m_pStream->write(str, size); 37 | } else { 38 | m_buffer.resize(std::max(m_buffer.size(), m_pos + size + 1)); 39 | std::copy(str, str + size, m_buffer.begin() + m_pos); 40 | } 41 | 42 | for (std::size_t i = 0; i < size; i++) { 43 | update_pos(str[i]); 44 | } 45 | } 46 | 47 | void ostream_wrapper::update_pos(char ch) { 48 | m_pos++; 49 | m_col++; 50 | 51 | if (ch == '\n') { 52 | m_row++; 53 | m_col = 0; 54 | m_comment = false; 55 | } 56 | } 57 | } 58 | -------------------------------------------------------------------------------- /cyberdog_gazebo/third-party/ParamHandler/src/parse.cpp: -------------------------------------------------------------------------------- 1 | #include "dynacore_yaml-cpp/node/parse.h" 2 | 3 | #include 4 | #include 5 | 6 | #include "dynacore_yaml-cpp/node/node.h" 7 | #include "dynacore_yaml-cpp/node/impl.h" 8 | #include "dynacore_yaml-cpp/parser.h" 9 | #include "nodebuilder.h" 10 | 11 | namespace dynacore_YAML { 12 | Node Load(const std::string& input) { 13 | std::stringstream stream(input); 14 | return Load(stream); 15 | } 16 | 17 | Node Load(const char* input) { 18 | std::stringstream stream(input); 19 | return Load(stream); 20 | } 21 | 22 | Node Load(std::istream& input) { 23 | Parser parser(input); 24 | NodeBuilder builder; 25 | if (!parser.HandleNextDocument(builder)) { 26 | return Node(); 27 | } 28 | 29 | return builder.Root(); 30 | } 31 | 32 | Node LoadFile(const std::string& filename) { 33 | std::ifstream fin(filename.c_str()); 34 | if (!fin) { 35 | throw BadFile(); 36 | } 37 | return Load(fin); 38 | } 39 | 40 | std::vector LoadAll(const std::string& input) { 41 | std::stringstream stream(input); 42 | return LoadAll(stream); 43 | } 44 | 45 | std::vector LoadAll(const char* input) { 46 | std::stringstream stream(input); 47 | return LoadAll(stream); 48 | } 49 | 50 | std::vector LoadAll(std::istream& input) { 51 | std::vector docs; 52 | 53 | Parser parser(input); 54 | while (1) { 55 | NodeBuilder builder; 56 | if (!parser.HandleNextDocument(builder)) { 57 | break; 58 | } 59 | docs.push_back(builder.Root()); 60 | } 61 | 62 | return docs; 63 | } 64 | 65 | std::vector LoadAllFromFile(const std::string& filename) { 66 | std::ifstream fin(filename.c_str()); 67 | if (!fin) { 68 | throw BadFile(); 69 | } 70 | return LoadAll(fin); 71 | } 72 | } // namespace YAML 73 | -------------------------------------------------------------------------------- /cyberdog_gazebo/third-party/ParamHandler/src/parser.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include "directives.h" // IWYU pragma: keep 5 | #include "scanner.h" // IWYU pragma: keep 6 | #include "singledocparser.h" 7 | #include "token.h" 8 | #include "dynacore_yaml-cpp/exceptions.h" // IWYU pragma: keep 9 | #include "dynacore_yaml-cpp/parser.h" 10 | 11 | namespace dynacore_YAML { 12 | class EventHandler; 13 | 14 | Parser::Parser() {} 15 | 16 | Parser::Parser(std::istream& in) { Load(in); } 17 | 18 | Parser::~Parser() {} 19 | 20 | Parser::operator bool() const { 21 | return m_pScanner.get() && !m_pScanner->empty(); 22 | } 23 | 24 | void Parser::Load(std::istream& in) { 25 | m_pScanner.reset(new Scanner(in)); 26 | m_pDirectives.reset(new Directives); 27 | } 28 | 29 | bool Parser::HandleNextDocument(EventHandler& eventHandler) { 30 | if (!m_pScanner.get()) 31 | return false; 32 | 33 | ParseDirectives(); 34 | if (m_pScanner->empty()) { 35 | return false; 36 | } 37 | 38 | SingleDocParser sdp(*m_pScanner, *m_pDirectives); 39 | sdp.HandleDocument(eventHandler); 40 | return true; 41 | } 42 | 43 | void Parser::ParseDirectives() { 44 | bool readDirective = false; 45 | 46 | while (1) { 47 | if (m_pScanner->empty()) { 48 | break; 49 | } 50 | 51 | Token& token = m_pScanner->peek(); 52 | if (token.type != Token::DIRECTIVE) { 53 | break; 54 | } 55 | 56 | // we keep the directives from the last document if none are specified; 57 | // but if any directives are specific, then we reset them 58 | if (!readDirective) { 59 | m_pDirectives.reset(new Directives); 60 | } 61 | 62 | readDirective = true; 63 | HandleDirective(token); 64 | m_pScanner->pop(); 65 | } 66 | } 67 | 68 | void Parser::HandleDirective(const Token& token) { 69 | if (token.value == "YAML") { 70 | HandleYamlDirective(token); 71 | } else if (token.value == "TAG") { 72 | HandleTagDirective(token); 73 | } 74 | } 75 | 76 | void Parser::HandleYamlDirective(const Token& token) { 77 | if (token.params.size() != 1) { 78 | throw ParserException(token.mark, ErrorMsg::YAML_DIRECTIVE_ARGS); 79 | } 80 | 81 | if (!m_pDirectives->version.isDefault) { 82 | throw ParserException(token.mark, ErrorMsg::REPEATED_YAML_DIRECTIVE); 83 | } 84 | 85 | std::stringstream str(token.params[0]); 86 | str >> m_pDirectives->version.major; 87 | str.get(); 88 | str >> m_pDirectives->version.minor; 89 | if (!str || str.peek() != EOF) { 90 | throw ParserException( 91 | token.mark, std::string(ErrorMsg::YAML_VERSION) + token.params[0]); 92 | } 93 | 94 | if (m_pDirectives->version.major > 1) { 95 | throw ParserException(token.mark, ErrorMsg::YAML_MAJOR_VERSION); 96 | } 97 | 98 | m_pDirectives->version.isDefault = false; 99 | // TODO: warning on major == 1, minor > 2? 100 | } 101 | 102 | void Parser::HandleTagDirective(const Token& token) { 103 | if (token.params.size() != 2) 104 | throw ParserException(token.mark, ErrorMsg::TAG_DIRECTIVE_ARGS); 105 | 106 | const std::string& handle = token.params[0]; 107 | const std::string& prefix = token.params[1]; 108 | if (m_pDirectives->tags.find(handle) != m_pDirectives->tags.end()) { 109 | throw ParserException(token.mark, ErrorMsg::REPEATED_TAG_DIRECTIVE); 110 | } 111 | 112 | m_pDirectives->tags[handle] = prefix; 113 | } 114 | 115 | void Parser::PrintTokens(std::ostream& out) { 116 | if (!m_pScanner.get()) { 117 | return; 118 | } 119 | 120 | while (1) { 121 | if (m_pScanner->empty()) { 122 | break; 123 | } 124 | 125 | out << m_pScanner->peek() << "\n"; 126 | m_pScanner->pop(); 127 | } 128 | } 129 | } 130 | -------------------------------------------------------------------------------- /cyberdog_gazebo/third-party/ParamHandler/src/ptr_vector.h: -------------------------------------------------------------------------------- 1 | #ifndef PTR_VECTOR_H_62B23520_7C8E_11DE_8A39_0800200C9A66 2 | #define PTR_VECTOR_H_62B23520_7C8E_11DE_8A39_0800200C9A66 3 | 4 | #if defined(_MSC_VER) || \ 5 | (defined(__GNUC__) && (__GNUC__ == 3 && __GNUC_MINOR__ >= 4) || \ 6 | (__GNUC__ >= 4)) // GCC supports "pragma once" correctly since 3.4 7 | #pragma once 8 | #endif 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | #include "dynacore_yaml-cpp/noncopyable.h" 16 | 17 | namespace dynacore_YAML { 18 | 19 | // TODO: This class is no longer needed 20 | template 21 | class ptr_vector : private dynacore_YAML::noncopyable { 22 | public: 23 | ptr_vector() {} 24 | 25 | void clear() { m_data.clear(); } 26 | 27 | std::size_t size() const { return m_data.size(); } 28 | bool empty() const { return m_data.empty(); } 29 | 30 | void push_back(std::unique_ptr&& t) { m_data.push_back(std::move(t)); } 31 | T& operator[](std::size_t i) { return *m_data[i]; } 32 | const T& operator[](std::size_t i) const { return *m_data[i]; } 33 | 34 | T& back() { return *(m_data.back().get()); } 35 | 36 | const T& back() const { return *(m_data.back().get()); } 37 | 38 | private: 39 | std::vector> m_data; 40 | }; 41 | } 42 | 43 | #endif // PTR_VECTOR_H_62B23520_7C8E_11DE_8A39_0800200C9A66 44 | -------------------------------------------------------------------------------- /cyberdog_gazebo/third-party/ParamHandler/src/regex_yaml.cpp: -------------------------------------------------------------------------------- 1 | #include "regex_yaml.h" 2 | 3 | namespace dynacore_YAML { 4 | // constructors 5 | RegEx::RegEx() : m_op(REGEX_EMPTY) {} 6 | 7 | RegEx::RegEx(REGEX_OP op) : m_op(op) {} 8 | 9 | RegEx::RegEx(char ch) : m_op(REGEX_MATCH), m_a(ch) {} 10 | 11 | RegEx::RegEx(char a, char z) : m_op(REGEX_RANGE), m_a(a), m_z(z) {} 12 | 13 | RegEx::RegEx(const std::string& str, REGEX_OP op) : m_op(op) { 14 | for (std::size_t i = 0; i < str.size(); i++) 15 | m_params.push_back(RegEx(str[i])); 16 | } 17 | 18 | // combination constructors 19 | RegEx operator!(const RegEx& ex) { 20 | RegEx ret(REGEX_NOT); 21 | ret.m_params.push_back(ex); 22 | return ret; 23 | } 24 | 25 | RegEx operator||(const RegEx& ex1, const RegEx& ex2) { 26 | RegEx ret(REGEX_OR); 27 | ret.m_params.push_back(ex1); 28 | ret.m_params.push_back(ex2); 29 | return ret; 30 | } 31 | 32 | RegEx operator&&(const RegEx& ex1, const RegEx& ex2) { 33 | RegEx ret(REGEX_AND); 34 | ret.m_params.push_back(ex1); 35 | ret.m_params.push_back(ex2); 36 | return ret; 37 | } 38 | 39 | RegEx operator+(const RegEx& ex1, const RegEx& ex2) { 40 | RegEx ret(REGEX_SEQ); 41 | ret.m_params.push_back(ex1); 42 | ret.m_params.push_back(ex2); 43 | return ret; 44 | } 45 | } 46 | -------------------------------------------------------------------------------- /cyberdog_gazebo/third-party/ParamHandler/src/regex_yaml.h: -------------------------------------------------------------------------------- 1 | #ifndef REGEX_H_62B23520_7C8E_11DE_8A39_0800200C9A66 2 | #define REGEX_H_62B23520_7C8E_11DE_8A39_0800200C9A66 3 | 4 | #if defined(_MSC_VER) || \ 5 | (defined(__GNUC__) && (__GNUC__ == 3 && __GNUC_MINOR__ >= 4) || \ 6 | (__GNUC__ >= 4)) // GCC supports "pragma once" correctly since 3.4 7 | #pragma once 8 | #endif 9 | 10 | #include 11 | #include 12 | 13 | #include "dynacore_yaml-cpp/dll.h" 14 | 15 | namespace dynacore_YAML { 16 | class Stream; 17 | 18 | enum REGEX_OP { 19 | REGEX_EMPTY, 20 | REGEX_MATCH, 21 | REGEX_RANGE, 22 | REGEX_OR, 23 | REGEX_AND, 24 | REGEX_NOT, 25 | REGEX_SEQ 26 | }; 27 | 28 | // simplified regular expressions 29 | // . Only straightforward matches (no repeated characters) 30 | // . Only matches from start of string 31 | class YAML_CPP_API RegEx { 32 | public: 33 | RegEx(); 34 | RegEx(char ch); 35 | RegEx(char a, char z); 36 | RegEx(const std::string& str, REGEX_OP op = REGEX_SEQ); 37 | ~RegEx() {} 38 | 39 | friend YAML_CPP_API RegEx operator!(const RegEx& ex); 40 | friend YAML_CPP_API RegEx operator||(const RegEx& ex1, const RegEx& ex2); 41 | friend YAML_CPP_API RegEx operator&&(const RegEx& ex1, const RegEx& ex2); 42 | friend YAML_CPP_API RegEx operator+(const RegEx& ex1, const RegEx& ex2); 43 | 44 | bool Matches(char ch) const; 45 | bool Matches(const std::string& str) const; 46 | bool Matches(const Stream& in) const; 47 | template 48 | bool Matches(const Source& source) const; 49 | 50 | int Match(const std::string& str) const; 51 | int Match(const Stream& in) const; 52 | template 53 | int Match(const Source& source) const; 54 | 55 | private: 56 | RegEx(REGEX_OP op); 57 | 58 | template 59 | bool IsValidSource(const Source& source) const; 60 | template 61 | int MatchUnchecked(const Source& source) const; 62 | 63 | template 64 | int MatchOpEmpty(const Source& source) const; 65 | template 66 | int MatchOpMatch(const Source& source) const; 67 | template 68 | int MatchOpRange(const Source& source) const; 69 | template 70 | int MatchOpOr(const Source& source) const; 71 | template 72 | int MatchOpAnd(const Source& source) const; 73 | template 74 | int MatchOpNot(const Source& source) const; 75 | template 76 | int MatchOpSeq(const Source& source) const; 77 | 78 | private: 79 | REGEX_OP m_op; 80 | char m_a, m_z; 81 | std::vector m_params; 82 | }; 83 | } 84 | 85 | #include "regeximpl.h" 86 | 87 | #endif // REGEX_H_62B23520_7C8E_11DE_8A39_0800200C9A66 88 | -------------------------------------------------------------------------------- /cyberdog_gazebo/third-party/ParamHandler/src/scanscalar.h: -------------------------------------------------------------------------------- 1 | #ifndef SCANSCALAR_H_62B23520_7C8E_11DE_8A39_0800200C9A66 2 | #define SCANSCALAR_H_62B23520_7C8E_11DE_8A39_0800200C9A66 3 | 4 | #if defined(_MSC_VER) || \ 5 | (defined(__GNUC__) && (__GNUC__ == 3 && __GNUC_MINOR__ >= 4) || \ 6 | (__GNUC__ >= 4)) // GCC supports "pragma once" correctly since 3.4 7 | #pragma once 8 | #endif 9 | 10 | #include 11 | 12 | #include "regex_yaml.h" 13 | #include "stream.h" 14 | 15 | namespace dynacore_YAML { 16 | enum CHOMP { STRIP = -1, CLIP, KEEP }; 17 | enum ACTION { NONE, BREAK, THROW }; 18 | enum FOLD { DONT_FOLD, FOLD_BLOCK, FOLD_FLOW }; 19 | 20 | struct ScanScalarParams { 21 | ScanScalarParams() 22 | : end(nullptr), 23 | eatEnd(false), 24 | indent(0), 25 | detectIndent(false), 26 | eatLeadingWhitespace(0), 27 | escape(0), 28 | fold(DONT_FOLD), 29 | trimTrailingSpaces(0), 30 | chomp(CLIP), 31 | onDocIndicator(NONE), 32 | onTabInIndentation(NONE), 33 | leadingSpaces(false) {} 34 | 35 | // input: 36 | const RegEx* end; // what condition ends this scalar? 37 | // unowned. 38 | bool eatEnd; // should we eat that condition when we see it? 39 | int indent; // what level of indentation should be eaten and ignored? 40 | bool detectIndent; // should we try to autodetect the indent? 41 | bool eatLeadingWhitespace; // should we continue eating this delicious 42 | // indentation after 'indent' spaces? 43 | char escape; // what character do we escape on (i.e., slash or single quote) 44 | // (0 for none) 45 | FOLD fold; // how do we fold line ends? 46 | bool trimTrailingSpaces; // do we remove all trailing spaces (at the very 47 | // end) 48 | CHOMP chomp; // do we strip, clip, or keep trailing newlines (at the very 49 | // end) 50 | // Note: strip means kill all, clip means keep at most one, keep means keep 51 | // all 52 | ACTION onDocIndicator; // what do we do if we see a document indicator? 53 | ACTION onTabInIndentation; // what do we do if we see a tab where we should 54 | // be seeing indentation spaces 55 | 56 | // output: 57 | bool leadingSpaces; 58 | }; 59 | 60 | std::string ScanScalar(Stream& INPUT, ScanScalarParams& info); 61 | } 62 | 63 | #endif // SCANSCALAR_H_62B23520_7C8E_11DE_8A39_0800200C9A66 64 | -------------------------------------------------------------------------------- /cyberdog_gazebo/third-party/ParamHandler/src/scantag.cpp: -------------------------------------------------------------------------------- 1 | #include "exp.h" 2 | #include "regex_yaml.h" 3 | #include "regeximpl.h" 4 | #include "stream.h" 5 | #include "dynacore_yaml-cpp/exceptions.h" // IWYU pragma: keep 6 | #include "dynacore_yaml-cpp/mark.h" 7 | 8 | namespace dynacore_YAML { 9 | const std::string ScanVerbatimTag(Stream& INPUT) { 10 | std::string tag; 11 | 12 | // eat the start character 13 | INPUT.get(); 14 | 15 | while (INPUT) { 16 | if (INPUT.peek() == Keys::VerbatimTagEnd) { 17 | // eat the end character 18 | INPUT.get(); 19 | return tag; 20 | } 21 | 22 | int n = Exp::URI().Match(INPUT); 23 | if (n <= 0) 24 | break; 25 | 26 | tag += INPUT.get(n); 27 | } 28 | 29 | throw ParserException(INPUT.mark(), ErrorMsg::END_OF_VERBATIM_TAG); 30 | } 31 | 32 | const std::string ScanTagHandle(Stream& INPUT, bool& canBeHandle) { 33 | std::string tag; 34 | canBeHandle = true; 35 | Mark firstNonWordChar; 36 | 37 | while (INPUT) { 38 | if (INPUT.peek() == Keys::Tag) { 39 | if (!canBeHandle) 40 | throw ParserException(firstNonWordChar, ErrorMsg::CHAR_IN_TAG_HANDLE); 41 | break; 42 | } 43 | 44 | int n = 0; 45 | if (canBeHandle) { 46 | n = Exp::Word().Match(INPUT); 47 | if (n <= 0) { 48 | canBeHandle = false; 49 | firstNonWordChar = INPUT.mark(); 50 | } 51 | } 52 | 53 | if (!canBeHandle) 54 | n = Exp::Tag().Match(INPUT); 55 | 56 | if (n <= 0) 57 | break; 58 | 59 | tag += INPUT.get(n); 60 | } 61 | 62 | return tag; 63 | } 64 | 65 | const std::string ScanTagSuffix(Stream& INPUT) { 66 | std::string tag; 67 | 68 | while (INPUT) { 69 | int n = Exp::Tag().Match(INPUT); 70 | if (n <= 0) 71 | break; 72 | 73 | tag += INPUT.get(n); 74 | } 75 | 76 | if (tag.empty()) 77 | throw ParserException(INPUT.mark(), ErrorMsg::TAG_WITH_NO_SUFFIX); 78 | 79 | return tag; 80 | } 81 | } 82 | -------------------------------------------------------------------------------- /cyberdog_gazebo/third-party/ParamHandler/src/scantag.h: -------------------------------------------------------------------------------- 1 | #ifndef SCANTAG_H_62B23520_7C8E_11DE_8A39_0800200C9A66 2 | #define SCANTAG_H_62B23520_7C8E_11DE_8A39_0800200C9A66 3 | 4 | #if defined(_MSC_VER) || \ 5 | (defined(__GNUC__) && (__GNUC__ == 3 && __GNUC_MINOR__ >= 4) || \ 6 | (__GNUC__ >= 4)) // GCC supports "pragma once" correctly since 3.4 7 | #pragma once 8 | #endif 9 | 10 | #include 11 | #include "stream.h" 12 | 13 | namespace dynacore_YAML { 14 | const std::string ScanVerbatimTag(Stream& INPUT); 15 | const std::string ScanTagHandle(Stream& INPUT, bool& canBeHandle); 16 | const std::string ScanTagSuffix(Stream& INPUT); 17 | } 18 | 19 | #endif // SCANTAG_H_62B23520_7C8E_11DE_8A39_0800200C9A66 20 | -------------------------------------------------------------------------------- /cyberdog_gazebo/third-party/ParamHandler/src/setting.h: -------------------------------------------------------------------------------- 1 | #ifndef SETTING_H_62B23520_7C8E_11DE_8A39_0800200C9A66 2 | #define SETTING_H_62B23520_7C8E_11DE_8A39_0800200C9A66 3 | 4 | #if defined(_MSC_VER) || \ 5 | (defined(__GNUC__) && (__GNUC__ == 3 && __GNUC_MINOR__ >= 4) || \ 6 | (__GNUC__ >= 4)) // GCC supports "pragma once" correctly since 3.4 7 | #pragma once 8 | #endif 9 | 10 | #include 11 | #include 12 | #include "dynacore_yaml-cpp/noncopyable.h" 13 | 14 | namespace dynacore_YAML { 15 | class SettingChangeBase; 16 | 17 | template 18 | class Setting { 19 | public: 20 | Setting() : m_value() {} 21 | 22 | const T get() const { return m_value; } 23 | std::unique_ptr set(const T& value); 24 | void restore(const Setting& oldSetting) { m_value = oldSetting.get(); } 25 | 26 | private: 27 | T m_value; 28 | }; 29 | 30 | class SettingChangeBase { 31 | public: 32 | virtual ~SettingChangeBase() {} 33 | virtual void pop() = 0; 34 | }; 35 | 36 | template 37 | class SettingChange : public SettingChangeBase { 38 | public: 39 | SettingChange(Setting* pSetting) : m_pCurSetting(pSetting) { 40 | // copy old setting to save its state 41 | m_oldSetting = *pSetting; 42 | } 43 | 44 | virtual void pop() { m_pCurSetting->restore(m_oldSetting); } 45 | 46 | private: 47 | Setting* m_pCurSetting; 48 | Setting m_oldSetting; 49 | }; 50 | 51 | template 52 | inline std::unique_ptr Setting::set(const T& value) { 53 | std::unique_ptr pChange(new SettingChange(this)); 54 | m_value = value; 55 | return pChange; 56 | } 57 | 58 | class SettingChanges : private noncopyable { 59 | public: 60 | SettingChanges() {} 61 | ~SettingChanges() { clear(); } 62 | 63 | void clear() { 64 | restore(); 65 | m_settingChanges.clear(); 66 | } 67 | 68 | void restore() { 69 | for (setting_changes::const_iterator it = m_settingChanges.begin(); 70 | it != m_settingChanges.end(); ++it) 71 | (*it)->pop(); 72 | } 73 | 74 | void push(std::unique_ptr pSettingChange) { 75 | m_settingChanges.push_back(std::move(pSettingChange)); 76 | } 77 | 78 | // like std::unique_ptr - assignment is transfer of ownership 79 | SettingChanges& operator=(SettingChanges&& rhs) { 80 | if (this == &rhs) 81 | return *this; 82 | 83 | clear(); 84 | std::swap(m_settingChanges, rhs.m_settingChanges); 85 | 86 | return *this; 87 | } 88 | 89 | private: 90 | typedef std::vector> setting_changes; 91 | setting_changes m_settingChanges; 92 | }; 93 | } 94 | 95 | #endif // SETTING_H_62B23520_7C8E_11DE_8A39_0800200C9A66 96 | -------------------------------------------------------------------------------- /cyberdog_gazebo/third-party/ParamHandler/src/simplekey.cpp: -------------------------------------------------------------------------------- 1 | #include "scanner.h" 2 | #include "token.h" 3 | 4 | namespace dynacore_YAML { 5 | struct Mark; 6 | 7 | Scanner::SimpleKey::SimpleKey(const Mark& mark_, std::size_t flowLevel_) 8 | : mark(mark_), flowLevel(flowLevel_), pIndent(0), pMapStart(0), pKey(0) {} 9 | 10 | void Scanner::SimpleKey::Validate() { 11 | // Note: pIndent will *not* be garbage here; 12 | // we "garbage collect" them so we can 13 | // always refer to them 14 | if (pIndent) 15 | pIndent->status = IndentMarker::VALID; 16 | if (pMapStart) 17 | pMapStart->status = Token::VALID; 18 | if (pKey) 19 | pKey->status = Token::VALID; 20 | } 21 | 22 | void Scanner::SimpleKey::Invalidate() { 23 | if (pIndent) 24 | pIndent->status = IndentMarker::INVALID; 25 | if (pMapStart) 26 | pMapStart->status = Token::INVALID; 27 | if (pKey) 28 | pKey->status = Token::INVALID; 29 | } 30 | 31 | // CanInsertPotentialSimpleKey 32 | bool Scanner::CanInsertPotentialSimpleKey() const { 33 | if (!m_simpleKeyAllowed) 34 | return false; 35 | 36 | return !ExistsActiveSimpleKey(); 37 | } 38 | 39 | // ExistsActiveSimpleKey 40 | // . Returns true if there's a potential simple key at our flow level 41 | // (there's allowed at most one per flow level, i.e., at the start of the flow 42 | // start token) 43 | bool Scanner::ExistsActiveSimpleKey() const { 44 | if (m_simpleKeys.empty()) 45 | return false; 46 | 47 | const SimpleKey& key = m_simpleKeys.top(); 48 | return key.flowLevel == GetFlowLevel(); 49 | } 50 | 51 | // InsertPotentialSimpleKey 52 | // . If we can, add a potential simple key to the queue, 53 | // and save it on a stack. 54 | void Scanner::InsertPotentialSimpleKey() { 55 | if (!CanInsertPotentialSimpleKey()) 56 | return; 57 | 58 | SimpleKey key(INPUT.mark(), GetFlowLevel()); 59 | 60 | // first add a map start, if necessary 61 | if (InBlockContext()) { 62 | key.pIndent = PushIndentTo(INPUT.column(), IndentMarker::MAP); 63 | if (key.pIndent) { 64 | key.pIndent->status = IndentMarker::UNKNOWN; 65 | key.pMapStart = key.pIndent->pStartToken; 66 | key.pMapStart->status = Token::UNVERIFIED; 67 | } 68 | } 69 | 70 | // then add the (now unverified) key 71 | m_tokens.push(Token(Token::KEY, INPUT.mark())); 72 | key.pKey = &m_tokens.back(); 73 | key.pKey->status = Token::UNVERIFIED; 74 | 75 | m_simpleKeys.push(key); 76 | } 77 | 78 | // InvalidateSimpleKey 79 | // . Automatically invalidate the simple key in our flow level 80 | void Scanner::InvalidateSimpleKey() { 81 | if (m_simpleKeys.empty()) 82 | return; 83 | 84 | // grab top key 85 | SimpleKey& key = m_simpleKeys.top(); 86 | if (key.flowLevel != GetFlowLevel()) 87 | return; 88 | 89 | key.Invalidate(); 90 | m_simpleKeys.pop(); 91 | } 92 | 93 | // VerifySimpleKey 94 | // . Determines whether the latest simple key to be added is valid, 95 | // and if so, makes it valid. 96 | bool Scanner::VerifySimpleKey() { 97 | if (m_simpleKeys.empty()) 98 | return false; 99 | 100 | // grab top key 101 | SimpleKey key = m_simpleKeys.top(); 102 | 103 | // only validate if we're in the correct flow level 104 | if (key.flowLevel != GetFlowLevel()) 105 | return false; 106 | 107 | m_simpleKeys.pop(); 108 | 109 | bool isValid = true; 110 | 111 | // needs to be less than 1024 characters and inline 112 | if (INPUT.line() != key.mark.line || INPUT.pos() - key.mark.pos > 1024) 113 | isValid = false; 114 | 115 | // invalidate key 116 | if (isValid) 117 | key.Validate(); 118 | else 119 | key.Invalidate(); 120 | 121 | return isValid; 122 | } 123 | 124 | void Scanner::PopAllSimpleKeys() { 125 | while (!m_simpleKeys.empty()) 126 | m_simpleKeys.pop(); 127 | } 128 | } 129 | -------------------------------------------------------------------------------- /cyberdog_gazebo/third-party/ParamHandler/src/singledocparser.h: -------------------------------------------------------------------------------- 1 | #ifndef SINGLEDOCPARSER_H_62B23520_7C8E_11DE_8A39_0800200C9A66 2 | #define SINGLEDOCPARSER_H_62B23520_7C8E_11DE_8A39_0800200C9A66 3 | 4 | #if defined(_MSC_VER) || \ 5 | (defined(__GNUC__) && (__GNUC__ == 3 && __GNUC_MINOR__ >= 4) || \ 6 | (__GNUC__ >= 4)) // GCC supports "pragma once" correctly since 3.4 7 | #pragma once 8 | #endif 9 | 10 | #include 11 | #include 12 | #include 13 | 14 | #include "dynacore_yaml-cpp/anchor.h" 15 | #include "dynacore_yaml-cpp/noncopyable.h" 16 | 17 | namespace dynacore_YAML { 18 | class CollectionStack; 19 | class EventHandler; 20 | class Node; 21 | class Scanner; 22 | struct Directives; 23 | struct Mark; 24 | struct Token; 25 | 26 | class SingleDocParser : private noncopyable { 27 | public: 28 | SingleDocParser(Scanner& scanner, const Directives& directives); 29 | ~SingleDocParser(); 30 | 31 | void HandleDocument(EventHandler& eventHandler); 32 | 33 | private: 34 | void HandleNode(EventHandler& eventHandler); 35 | 36 | void HandleSequence(EventHandler& eventHandler); 37 | void HandleBlockSequence(EventHandler& eventHandler); 38 | void HandleFlowSequence(EventHandler& eventHandler); 39 | 40 | void HandleMap(EventHandler& eventHandler); 41 | void HandleBlockMap(EventHandler& eventHandler); 42 | void HandleFlowMap(EventHandler& eventHandler); 43 | void HandleCompactMap(EventHandler& eventHandler); 44 | void HandleCompactMapWithNoKey(EventHandler& eventHandler); 45 | 46 | void ParseProperties(std::string& tag, anchor_t& anchor); 47 | void ParseTag(std::string& tag); 48 | void ParseAnchor(anchor_t& anchor); 49 | 50 | anchor_t RegisterAnchor(const std::string& name); 51 | anchor_t LookupAnchor(const Mark& mark, const std::string& name) const; 52 | 53 | private: 54 | Scanner& m_scanner; 55 | const Directives& m_directives; 56 | std::unique_ptr m_pCollectionStack; 57 | 58 | typedef std::map Anchors; 59 | Anchors m_anchors; 60 | 61 | anchor_t m_curAnchor; 62 | }; 63 | } 64 | 65 | #endif // SINGLEDOCPARSER_H_62B23520_7C8E_11DE_8A39_0800200C9A66 66 | -------------------------------------------------------------------------------- /cyberdog_gazebo/third-party/ParamHandler/src/stream.h: -------------------------------------------------------------------------------- 1 | #ifndef STREAM_H_62B23520_7C8E_11DE_8A39_0800200C9A66 2 | #define STREAM_H_62B23520_7C8E_11DE_8A39_0800200C9A66 3 | 4 | #if defined(_MSC_VER) || \ 5 | (defined(__GNUC__) && (__GNUC__ == 3 && __GNUC_MINOR__ >= 4) || \ 6 | (__GNUC__ >= 4)) // GCC supports "pragma once" correctly since 3.4 7 | #pragma once 8 | #endif 9 | 10 | #include "dynacore_yaml-cpp/noncopyable.h" 11 | #include "dynacore_yaml-cpp/mark.h" 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | namespace dynacore_YAML { 20 | class Stream : private noncopyable { 21 | public: 22 | friend class StreamCharSource; 23 | 24 | Stream(std::istream& input); 25 | ~Stream(); 26 | 27 | operator bool() const; 28 | bool operator!() const { return !static_cast(*this); } 29 | 30 | char peek() const; 31 | char get(); 32 | std::string get(int n); 33 | void eat(int n = 1); 34 | 35 | static char eof() { return 0x04; } 36 | 37 | const Mark mark() const { return m_mark; } 38 | int pos() const { return m_mark.pos; } 39 | int line() const { return m_mark.line; } 40 | int column() const { return m_mark.column; } 41 | void ResetColumn() { m_mark.column = 0; } 42 | 43 | private: 44 | enum CharacterSet { utf8, utf16le, utf16be, utf32le, utf32be }; 45 | 46 | std::istream& m_input; 47 | Mark m_mark; 48 | 49 | CharacterSet m_charSet; 50 | mutable std::deque m_readahead; 51 | unsigned char* const m_pPrefetched; 52 | mutable size_t m_nPrefetchedAvailable; 53 | mutable size_t m_nPrefetchedUsed; 54 | 55 | void AdvanceCurrent(); 56 | char CharAt(size_t i) const; 57 | bool ReadAheadTo(size_t i) const; 58 | bool _ReadAheadTo(size_t i) const; 59 | void StreamInUtf8() const; 60 | void StreamInUtf16() const; 61 | void StreamInUtf32() const; 62 | unsigned char GetNextByte() const; 63 | }; 64 | 65 | // CharAt 66 | // . Unchecked access 67 | inline char Stream::CharAt(size_t i) const { return m_readahead[i]; } 68 | 69 | inline bool Stream::ReadAheadTo(size_t i) const { 70 | if (m_readahead.size() > i) 71 | return true; 72 | return _ReadAheadTo(i); 73 | } 74 | } 75 | 76 | #endif // STREAM_H_62B23520_7C8E_11DE_8A39_0800200C9A66 77 | -------------------------------------------------------------------------------- /cyberdog_gazebo/third-party/ParamHandler/src/streamcharsource.h: -------------------------------------------------------------------------------- 1 | #ifndef STREAMCHARSOURCE_H_62B23520_7C8E_11DE_8A39_0800200C9A66 2 | #define STREAMCHARSOURCE_H_62B23520_7C8E_11DE_8A39_0800200C9A66 3 | 4 | #if defined(_MSC_VER) || \ 5 | (defined(__GNUC__) && (__GNUC__ == 3 && __GNUC_MINOR__ >= 4) || \ 6 | (__GNUC__ >= 4)) // GCC supports "pragma once" correctly since 3.4 7 | #pragma once 8 | #endif 9 | 10 | #include "dynacore_yaml-cpp/noncopyable.h" 11 | #include 12 | 13 | namespace dynacore_YAML { 14 | class StreamCharSource { 15 | public: 16 | StreamCharSource(const Stream& stream) : m_offset(0), m_stream(stream) {} 17 | StreamCharSource(const StreamCharSource& source) 18 | : m_offset(source.m_offset), m_stream(source.m_stream) {} 19 | ~StreamCharSource() {} 20 | 21 | operator bool() const; 22 | char operator[](std::size_t i) const { return m_stream.CharAt(m_offset + i); } 23 | bool operator!() const { return !static_cast(*this); } 24 | 25 | const StreamCharSource operator+(int i) const; 26 | 27 | private: 28 | std::size_t m_offset; 29 | const Stream& m_stream; 30 | 31 | StreamCharSource& operator=(const StreamCharSource&); // non-assignable 32 | }; 33 | 34 | inline StreamCharSource::operator bool() const { 35 | return m_stream.ReadAheadTo(m_offset); 36 | } 37 | 38 | inline const StreamCharSource StreamCharSource::operator+(int i) const { 39 | StreamCharSource source(*this); 40 | if (static_cast(source.m_offset) + i >= 0) 41 | source.m_offset += i; 42 | else 43 | source.m_offset = 0; 44 | return source; 45 | } 46 | } 47 | 48 | #endif // STREAMCHARSOURCE_H_62B23520_7C8E_11DE_8A39_0800200C9A66 49 | -------------------------------------------------------------------------------- /cyberdog_gazebo/third-party/ParamHandler/src/stringsource.h: -------------------------------------------------------------------------------- 1 | #ifndef STRINGSOURCE_H_62B23520_7C8E_11DE_8A39_0800200C9A66 2 | #define STRINGSOURCE_H_62B23520_7C8E_11DE_8A39_0800200C9A66 3 | 4 | #if defined(_MSC_VER) || \ 5 | (defined(__GNUC__) && (__GNUC__ == 3 && __GNUC_MINOR__ >= 4) || \ 6 | (__GNUC__ >= 4)) // GCC supports "pragma once" correctly since 3.4 7 | #pragma once 8 | #endif 9 | 10 | #include 11 | 12 | namespace dynacore_YAML { 13 | class StringCharSource { 14 | public: 15 | StringCharSource(const char* str, std::size_t size) 16 | : m_str(str), m_size(size), m_offset(0) {} 17 | 18 | operator bool() const { return m_offset < m_size; } 19 | char operator[](std::size_t i) const { return m_str[m_offset + i]; } 20 | bool operator!() const { return !static_cast(*this); } 21 | 22 | const StringCharSource operator+(int i) const { 23 | StringCharSource source(*this); 24 | if (static_cast(source.m_offset) + i >= 0) 25 | source.m_offset += i; 26 | else 27 | source.m_offset = 0; 28 | return source; 29 | } 30 | 31 | StringCharSource& operator++() { 32 | ++m_offset; 33 | return *this; 34 | } 35 | 36 | StringCharSource& operator+=(std::size_t offset) { 37 | m_offset += offset; 38 | return *this; 39 | } 40 | 41 | private: 42 | const char* m_str; 43 | std::size_t m_size; 44 | std::size_t m_offset; 45 | }; 46 | } 47 | 48 | #endif // STRINGSOURCE_H_62B23520_7C8E_11DE_8A39_0800200C9A66 49 | -------------------------------------------------------------------------------- /cyberdog_gazebo/third-party/ParamHandler/src/tag.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include "directives.h" // IWYU pragma: keep 5 | #include "tag.h" 6 | #include "token.h" 7 | 8 | namespace dynacore_YAML { 9 | Tag::Tag(const Token& token) : type(static_cast(token.data)) { 10 | switch (type) { 11 | case VERBATIM: 12 | value = token.value; 13 | break; 14 | case PRIMARY_HANDLE: 15 | value = token.value; 16 | break; 17 | case SECONDARY_HANDLE: 18 | value = token.value; 19 | break; 20 | case NAMED_HANDLE: 21 | handle = token.value; 22 | value = token.params[0]; 23 | break; 24 | case NON_SPECIFIC: 25 | break; 26 | default: 27 | assert(false); 28 | } 29 | } 30 | 31 | const std::string Tag::Translate(const Directives& directives) { 32 | switch (type) { 33 | case VERBATIM: 34 | return value; 35 | case PRIMARY_HANDLE: 36 | return directives.TranslateTagHandle("!") + value; 37 | case SECONDARY_HANDLE: 38 | return directives.TranslateTagHandle("!!") + value; 39 | case NAMED_HANDLE: 40 | return directives.TranslateTagHandle("!" + handle + "!") + value; 41 | case NON_SPECIFIC: 42 | // TODO: 43 | return "!"; 44 | default: 45 | assert(false); 46 | } 47 | throw std::runtime_error("yaml-cpp: internal error, bad tag type"); 48 | } 49 | } 50 | -------------------------------------------------------------------------------- /cyberdog_gazebo/third-party/ParamHandler/src/tag.h: -------------------------------------------------------------------------------- 1 | #ifndef TAG_H_62B23520_7C8E_11DE_8A39_0800200C9A66 2 | #define TAG_H_62B23520_7C8E_11DE_8A39_0800200C9A66 3 | 4 | #if defined(_MSC_VER) || \ 5 | (defined(__GNUC__) && (__GNUC__ == 3 && __GNUC_MINOR__ >= 4) || \ 6 | (__GNUC__ >= 4)) // GCC supports "pragma once" correctly since 3.4 7 | #pragma once 8 | #endif 9 | 10 | #include 11 | 12 | namespace dynacore_YAML { 13 | struct Directives; 14 | struct Token; 15 | 16 | struct Tag { 17 | enum TYPE { 18 | VERBATIM, 19 | PRIMARY_HANDLE, 20 | SECONDARY_HANDLE, 21 | NAMED_HANDLE, 22 | NON_SPECIFIC 23 | }; 24 | 25 | Tag(const Token& token); 26 | const std::string Translate(const Directives& directives); 27 | 28 | TYPE type; 29 | std::string handle, value; 30 | }; 31 | } 32 | 33 | #endif // TAG_H_62B23520_7C8E_11DE_8A39_0800200C9A66 34 | -------------------------------------------------------------------------------- /cyberdog_gazebo/third-party/ParamHandler/src/token.h: -------------------------------------------------------------------------------- 1 | #ifndef TOKEN_H_62B23520_7C8E_11DE_8A39_0800200C9A66 2 | #define TOKEN_H_62B23520_7C8E_11DE_8A39_0800200C9A66 3 | 4 | #if defined(_MSC_VER) || \ 5 | (defined(__GNUC__) && (__GNUC__ == 3 && __GNUC_MINOR__ >= 4) || \ 6 | (__GNUC__ >= 4)) // GCC supports "pragma once" correctly since 3.4 7 | #pragma once 8 | #endif 9 | 10 | #include "dynacore_yaml-cpp/mark.h" 11 | #include 12 | #include 13 | #include 14 | 15 | namespace dynacore_YAML { 16 | const std::string TokenNames[] = { 17 | "DIRECTIVE", "DOC_START", "DOC_END", "BLOCK_SEQ_START", "BLOCK_MAP_START", 18 | "BLOCK_SEQ_END", "BLOCK_MAP_END", "BLOCK_ENTRY", "FLOW_SEQ_START", 19 | "FLOW_MAP_START", "FLOW_SEQ_END", "FLOW_MAP_END", "FLOW_MAP_COMPACT", 20 | "FLOW_ENTRY", "KEY", "VALUE", "ANCHOR", "ALIAS", "TAG", "SCALAR"}; 21 | 22 | struct Token { 23 | // enums 24 | enum STATUS { VALID, INVALID, UNVERIFIED }; 25 | enum TYPE { 26 | DIRECTIVE, 27 | DOC_START, 28 | DOC_END, 29 | BLOCK_SEQ_START, 30 | BLOCK_MAP_START, 31 | BLOCK_SEQ_END, 32 | BLOCK_MAP_END, 33 | BLOCK_ENTRY, 34 | FLOW_SEQ_START, 35 | FLOW_MAP_START, 36 | FLOW_SEQ_END, 37 | FLOW_MAP_END, 38 | FLOW_MAP_COMPACT, 39 | FLOW_ENTRY, 40 | KEY, 41 | VALUE, 42 | ANCHOR, 43 | ALIAS, 44 | TAG, 45 | PLAIN_SCALAR, 46 | NON_PLAIN_SCALAR 47 | }; 48 | 49 | // data 50 | Token(TYPE type_, const Mark& mark_) 51 | : status(VALID), type(type_), mark(mark_), data(0) {} 52 | 53 | friend std::ostream& operator<<(std::ostream& out, const Token& token) { 54 | out << TokenNames[token.type] << std::string(": ") << token.value; 55 | for (std::size_t i = 0; i < token.params.size(); i++) 56 | out << std::string(" ") << token.params[i]; 57 | return out; 58 | } 59 | 60 | STATUS status; 61 | TYPE type; 62 | Mark mark; 63 | std::string value; 64 | std::vector params; 65 | int data; 66 | }; 67 | } 68 | 69 | #endif // TOKEN_H_62B23520_7C8E_11DE_8A39_0800200C9A66 70 | -------------------------------------------------------------------------------- /cyberdog_gazebo/world/heightmap.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 0.001 6 | 1 7 | 1000 8 | 0 0 -9.81 9 | 10 | 11 | quick 12 | 0.0001 13 | 50 14 | 1.3 15 | 16 | 17 | 0.0 18 | 0.2 19 | 20 | 21 | 22 | 23 | 24 | model://sun 25 | 26 | 27 | true 28 | 29 | 30 | 31 | 32 | file://media/materials/textures/terrain.png 33 | 8 8 0.5 34 | 0 0 0 35 | 36 | 37 | 38 | 39 | 40 | 41 | false 42 | 43 | file://media/materials/textures/dirt_diffusespecular.png 44 | file://media/materials/textures/flat_normal.png 45 | 1 46 | 47 | 48 | file://media/materials/textures/grass_diffusespecular.png 49 | file://media/materials/textures/flat_normal.png 50 | 1 51 | 52 | 53 | file://media/materials/textures/fungus_diffusespecular.png 54 | file://media/materials/textures/flat_normal.png 55 | 1 56 | 57 | 58 | 2 59 | 5 60 | 61 | 62 | 4 63 | 5 64 | 65 | file://media/materials/textures/terrain.png 66 | 8 8 0.5 67 | 0.0 0 0 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | -------------------------------------------------------------------------------- /cyberdog_gazebo/world/simple.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 0.001 7 | 1 8 | 1000 9 | 0 0 -9.81 10 | 11 | 12 | world 13 | 0.0001 14 | 50 15 | 1.3 16 | 17 | 18 | 0.0 19 | 0.2 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | model://sun 31 | 32 | 33 | 34 | 35 | model://ground_plane 36 | 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /cyberdog_msg/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(cyberdog_msg) 3 | 4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 5 | add_compile_options(-Wall -Wextra -Wpedantic) 6 | endif() 7 | 8 | # find dependencies 9 | find_package(ament_cmake REQUIRED) 10 | find_package(std_msgs REQUIRED) 11 | find_package(rosidl_default_generators REQUIRED) 12 | 13 | rosidl_generate_interfaces(${PROJECT_NAME} 14 | "msg/YamlParam.msg" 15 | "msg/ApplyForce.msg" 16 | DEPENDENCIES std_msgs 17 | ) 18 | 19 | ament_package() 20 | -------------------------------------------------------------------------------- /cyberdog_msg/msg/ApplyForce.msg: -------------------------------------------------------------------------------- 1 | string link_name 2 | float64 time 3 | float64[3] force 4 | float64[3] rel_pos 5 | -------------------------------------------------------------------------------- /cyberdog_msg/msg/YamlParam.msg: -------------------------------------------------------------------------------- 1 | uint64 DOUBLE = 1 2 | uint64 S64 = 2 3 | uint64 VEC_X_DOUBLE = 3 4 | uint64 MAT_X_DOUBLE = 4 5 | 6 | string name 7 | uint64 kind 8 | int64 s64_value 9 | float64 double_value 10 | float64[12] vecxd_value 11 | int64 is_user -------------------------------------------------------------------------------- /cyberdog_msg/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | cyberdog_msg 5 | 0.0.0 6 | TODO: Package description 7 | ljh 8 | TODO: License declaration 9 | 10 | ament_cmake 11 | rosidl_default_generators 12 | 13 | std_msgs 14 | 15 | rosidl_default_runtime 16 | 17 | rosidl_interface_packages 18 | 19 | 20 | ament_cmake 21 | 22 | 23 | -------------------------------------------------------------------------------- /cyberdog_robot/cyberdog_description/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(cyberdog_description) 3 | # Default to C99 4 | if(NOT CMAKE_C_STANDARD) 5 | set(CMAKE_C_STANDARD 99) 6 | endif() 7 | 8 | # Default to C++14 9 | if(NOT CMAKE_CXX_STANDARD) 10 | set(CMAKE_CXX_STANDARD 14) 11 | endif() 12 | 13 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 14 | add_compile_options(-Wall -Wextra -Wpedantic) 15 | endif() 16 | 17 | # find dependencies 18 | find_package(ament_cmake REQUIRED) 19 | find_package(rosidl_default_generators REQUIRED) 20 | find_package(rosidl_typesupport_cpp REQUIRED) 21 | find_package(rclcpp REQUIRED) 22 | find_package(rclcpp_components REQUIRED) 23 | find_package(std_msgs REQUIRED) 24 | find_package(tf2 REQUIRED) 25 | find_package(tf2_ros REQUIRED) 26 | find_package(tf2_geometry_msgs REQUIRED) 27 | 28 | install( 29 | DIRECTORY 30 | launch 31 | xacro 32 | meshes 33 | rviz 34 | DESTINATION share/${PROJECT_NAME} 35 | ) 36 | 37 | ament_package() 38 | -------------------------------------------------------------------------------- /cyberdog_robot/cyberdog_description/launch/cyberdog.launch.py: -------------------------------------------------------------------------------- 1 | 2 | import os 3 | import launch 4 | import xacro 5 | from launch.actions import DeclareLaunchArgument 6 | from launch.substitutions import LaunchConfiguration 7 | from ament_index_python.packages import get_package_share_directory 8 | from launch_ros.actions import Node 9 | from launch.actions import OpaqueFunction 10 | 11 | 12 | def launch_setup(context, *args, **kwargs): 13 | # package 14 | pkg_dir = get_package_share_directory('cyberdog') 15 | 16 | # urdf 17 | xacro_path = os.path.join(get_package_share_directory( 18 | 'cyberdog'), 'xacro', 'robot.xacro') 19 | urdf_contents = xacro.process_file(xacro_path).toprettyxml(indent=' ') 20 | 21 | joint_state_publisher_node = Node( 22 | package='joint_state_publisher_gui', 23 | executable='joint_state_publisher_gui', 24 | name='joint_state_publisher_gui', 25 | parameters=[ 26 | { 27 | 'robot_description': urdf_contents 28 | } 29 | ] 30 | ) 31 | 32 | robot_state_publisher_node = Node( 33 | package='robot_state_publisher', 34 | executable='robot_state_publisher', 35 | name='robot_state_publisher', 36 | parameters=[ 37 | { 38 | 'use_sim_time': LaunchConfiguration('use_sim_time') 39 | }, 40 | { 41 | 'robot_description': urdf_contents 42 | }, 43 | { 44 | 'publish_frequency': 500.0 45 | } 46 | ] 47 | ) 48 | 49 | rviz2_node = Node( 50 | package='rviz2', 51 | executable='rviz2', 52 | name='rviz2', 53 | output='screen', 54 | arguments=['-d', [os.path.join(pkg_dir, 'rviz', 'cyberdog.rviz')]] 55 | ) 56 | 57 | return [joint_state_publisher_node, robot_state_publisher_node, rviz2_node] 58 | 59 | def generate_launch_description(): 60 | return launch.LaunchDescription([ 61 | DeclareLaunchArgument( 62 | name='use_sim_time', 63 | default_value='true' 64 | ), 65 | OpaqueFunction(function=launch_setup) 66 | ]) 67 | -------------------------------------------------------------------------------- /cyberdog_robot/cyberdog_description/launch/cyberdog_description.launch.py: -------------------------------------------------------------------------------- 1 | 2 | import os 3 | import launch 4 | import xacro 5 | from launch.actions import DeclareLaunchArgument 6 | from launch.substitutions import LaunchConfiguration 7 | from ament_index_python.packages import get_package_share_directory 8 | from launch_ros.actions import Node 9 | from launch.actions import OpaqueFunction 10 | 11 | 12 | def launch_setup(context, *args, **kwargs): 13 | # package 14 | pkg_dir = get_package_share_directory('cyberdog_description') 15 | 16 | # urdf 17 | xacro_path = os.path.join(get_package_share_directory( 18 | 'cyberdog_description'), 'xacro', 'robot.xacro') 19 | urdf_contents = xacro.process_file(xacro_path).toprettyxml(indent=' ') 20 | 21 | joint_state_publisher_node = Node( 22 | package='joint_state_publisher_gui', 23 | executable='joint_state_publisher_gui', 24 | name='joint_state_publisher_gui', 25 | parameters=[ 26 | { 27 | 'robot_description': urdf_contents 28 | } 29 | ] 30 | ) 31 | 32 | robot_state_publisher_node = Node( 33 | package='robot_state_publisher', 34 | executable='robot_state_publisher', 35 | name='robot_state_publisher', 36 | parameters=[ 37 | { 38 | 'use_sim_time': LaunchConfiguration('use_sim_time') 39 | }, 40 | { 41 | 'robot_description': urdf_contents 42 | }, 43 | { 44 | 'publish_frequency': 500.0 45 | } 46 | ] 47 | ) 48 | 49 | rviz2_node = Node( 50 | package='rviz2', 51 | executable='rviz2', 52 | name='rviz2', 53 | output='screen', 54 | arguments=['-d', [os.path.join(pkg_dir, 'rviz', 'cyberdog.rviz')]] 55 | ) 56 | 57 | return [joint_state_publisher_node, robot_state_publisher_node, rviz2_node] 58 | 59 | def generate_launch_description(): 60 | return launch.LaunchDescription([ 61 | DeclareLaunchArgument( 62 | name='use_sim_time', 63 | default_value='true' 64 | ), 65 | OpaqueFunction(function=launch_setup) 66 | ]) 67 | -------------------------------------------------------------------------------- /cyberdog_robot/cyberdog_description/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | cyberdog_description 5 | 0.0.0 6 | The cyberdog package 7 | 8 | You Yangwei 9 | Sun Chenguang 10 | TODO 11 | 12 | ament_cmake 13 | rclcpp 14 | std_msgs 15 | 16 | 17 | ament_cmake 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /cyberdog_robot/cyberdog_description/xacro/transmission.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | transmission_interface/SimpleTransmission 8 | 9 | hardware_interface/EffortJointInterface 10 | 11 | 12 | hardware_interface/EffortJointInterface 13 | "${abadGearRatio}" 14 | 15 | 16 | 17 | 18 | transmission_interface/SimpleTransmission 19 | 20 | hardware_interface/EffortJointInterface 21 | 22 | 23 | hardware_interface/EffortJointInterface 24 | "${abadGearRatio}" 25 | 26 | 27 | 28 | 29 | transmission_interface/SimpleTransmission 30 | 31 | hardware_interface/EffortJointInterface 32 | 33 | 34 | hardware_interface/EffortJointInterface 35 | "${kneeGearRatio}" 36 | 37 | 38 | 39 | 40 | 41 | 42 | -------------------------------------------------------------------------------- /cyberdog_simulator/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(cyberdog_simulator) 3 | 4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 5 | add_compile_options(-Wall -Wextra -Wpedantic) 6 | endif() 7 | 8 | # find dependencies 9 | find_package(ament_cmake REQUIRED) 10 | 11 | ament_package() 12 | -------------------------------------------------------------------------------- /cyberdog_simulator/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | cyberdog_simulator 5 | 0.0.0 6 | package for building all the projects 7 | ljh 8 | Apache License, Version 2.0 9 | 10 | ament_cmake 11 | 12 | cyberdog_msg 13 | cyberdog_gazebo 14 | cyberdog_description 15 | cyberdog_visual 16 | 17 | cyberdog_example 18 | 19 | ament_lint_auto 20 | ament_lint_common 21 | 22 | 23 | ament_cmake 24 | 25 | 26 | -------------------------------------------------------------------------------- /cyberdog_visual/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(cyberdog_visual) 3 | # Default to C99 4 | if(NOT CMAKE_C_STANDARD) 5 | set(CMAKE_C_STANDARD 99) 6 | endif() 7 | 8 | # Default to C++14 9 | if(NOT CMAKE_CXX_STANDARD) 10 | set(CMAKE_CXX_STANDARD 14) 11 | endif() 12 | 13 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 14 | add_compile_options(-Wall -Wextra -Wpedantic) 15 | endif() 16 | 17 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") 18 | 19 | find_package(rclcpp REQUIRED) 20 | find_package(tf2 REQUIRED) 21 | find_package(tf2_eigen REQUIRED) 22 | find_package(tf2_geometry_msgs REQUIRED) 23 | find_package(tf2_ros REQUIRED) 24 | find_package(sensor_msgs REQUIRED) 25 | find_package(gazebo_msgs REQUIRED) 26 | find_package(lcm REQUIRED) 27 | find_package(urdf REQUIRED) 28 | set(dependencies 29 | rclcpp 30 | std_msgs 31 | tf2 32 | tf2_eigen 33 | tf2_geometry_msgs 34 | tf2_ros 35 | sensor_msgs 36 | gazebo_msgs 37 | urdf 38 | lcm 39 | Eigen3 40 | ) 41 | 42 | include_directories( 43 | include 44 | lcm_type 45 | ) 46 | 47 | add_executable(cyberdog_visual 48 | src/cyberdog_visual_node.cpp 49 | src/cyberdog_visual.cpp 50 | ) 51 | ament_target_dependencies(cyberdog_visual ${dependencies}) 52 | target_link_libraries(cyberdog_visual lcm) 53 | 54 | install(TARGETS cyberdog_visual 55 | EXPORT export_${PROJECT_NAME} 56 | RUNTIME DESTINATION lib/${PROJECT_NAME} 57 | ) 58 | install( 59 | DIRECTORY 60 | launch 61 | rviz 62 | DESTINATION share/${PROJECT_NAME} 63 | ) 64 | 65 | ament_package() -------------------------------------------------------------------------------- /cyberdog_visual/include/cyberdog_visual/cyberdog_visual.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023-2023 Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #pragma once 16 | 17 | #include 18 | #include 19 | #include "rclcpp/rclcpp.hpp" 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include "leg_control_data_lcmt.hpp" 31 | #include "state_estimator_lcmt.hpp" 32 | #include "simulator_lcmt.hpp" 33 | #include "localization_lcmt.hpp" 34 | 35 | namespace cyberdog 36 | { 37 | 38 | class CyberDogVisual : public rclcpp::Node 39 | { 40 | public: 41 | /** 42 | * @brief Construct a new CyberDogVisual object 43 | * 44 | */ 45 | CyberDogVisual(); 46 | 47 | /** 48 | * @brief Destroy the CyberDogVisual object 49 | * 50 | */ 51 | virtual ~CyberDogVisual(); 52 | 53 | private: 54 | /** 55 | * @brief update function for sending lcm messages 56 | * 57 | */ 58 | void UpdateTimerCallback(); 59 | 60 | /** 61 | * @brief Handle Odom lcm messages 62 | * 63 | * @param rbuf : buffer receive lcm messages 64 | * @param chan : the channel to subscribe the lcm message 65 | * @param msg : the Odom message 66 | */ 67 | void HandleOdomMessage(const lcm::ReceiveBuffer *rbuf, const std::string &chan, const state_estimator_lcmt *msg); 68 | 69 | /** 70 | * @brief Handle global to robot lcm messages 71 | * 72 | * @param rbuf : buffer receive lcm messages 73 | * @param chan : the channel to subscribe the lcm message 74 | * @param msg : the global to robot message 75 | */ 76 | void HandleGlobalOdomMessage(const lcm::ReceiveBuffer *rbuf, const std::string &chan, const localization_lcmt *msg); 77 | 78 | /** 79 | * @brief Handle joint states messages 80 | * 81 | * @param rbuf : buffer receive lcm messages 82 | * @param chan : the channel to subscribe the lcm message 83 | * @param msg : the joint states message 84 | */ 85 | void HandleJointMessage(const lcm::ReceiveBuffer *rbuf, const std::string &chan, const leg_control_data_lcmt *msg); 86 | 87 | /** 88 | * @brief Read parameters 89 | * 90 | */ 91 | void ReadParameters(); 92 | 93 | /** 94 | * @brief Get the name of joint 95 | * 96 | * @param urdf_model : the urdf of robot 97 | * @return std::vector : name list of each joints 98 | */ 99 | std::vector GetJointName(const urdf::Model &urdf_model); 100 | 101 | rclcpp::TimerBase::SharedPtr update_timer_; 102 | std::shared_ptr br_; 103 | rclcpp::Publisher::SharedPtr js_pub_; 104 | sensor_msgs::msg::JointState js_; 105 | 106 | lcm::LCM odom_lcm_; 107 | lcm::LCM odom_global_lcm_; 108 | lcm::LCM joint_lcm_; 109 | 110 | std::string joint_state_topic_; 111 | std::string robot_description_; 112 | std::string root_link_; 113 | std::string urdf_string; 114 | double publish_frequency_; 115 | 116 | bool use_state_estimator_; 117 | }; 118 | } -------------------------------------------------------------------------------- /cyberdog_visual/launch/cyberdog_lcm_repaly.launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | import launch 3 | import xacro 4 | from launch.actions import DeclareLaunchArgument 5 | from launch.actions import OpaqueFunction 6 | from launch.substitutions import Command, LaunchConfiguration, PythonExpression 7 | from launch_ros.actions import Node 8 | from launch_ros.substitutions import FindPackageShare 9 | 10 | def launch_setup(context, *args, **kwargs): 11 | # config 12 | hang_robot = LaunchConfiguration('hang_robot').perform(context) 13 | use_lidar = LaunchConfiguration('use_lidar').perform(context) 14 | rname = LaunchConfiguration('rname').perform(context) 15 | use_sim_time=LaunchConfiguration('use_sim_time') 16 | 17 | # path 18 | description_share = FindPackageShare( 19 | package=rname+'_description').find(rname+'_description') 20 | visual_share = FindPackageShare( 21 | package='cyberdog_visual').find('cyberdog_visual') 22 | 23 | # urdf 24 | xacro_path = os.path.join(description_share, 'xacro/robot.xacro') 25 | urdf_contents = xacro.process_file(xacro_path, mappings={ 26 | 'DEBUG': hang_robot, 'USE_LIDAR': use_lidar}).toprettyxml(indent=' ') 27 | 28 | # joint_state_publisher 29 | joint_state_node = Node( 30 | package='cyberdog_visual', 31 | executable='cyberdog_visual', 32 | name='cyberdog_visual', 33 | output='screen', 34 | parameters=[{ 35 | 'robot_description': urdf_contents, 36 | 'publish_frequency': 500.0, 37 | 'joint_state_topic': 'joint_states', 38 | 'use_sim_time': use_sim_time, 39 | 'use_state_estimator': False 40 | } 41 | ] 42 | ) 43 | 44 | # robot_state_publisher 45 | robot_state_node = Node( 46 | package='robot_state_publisher', 47 | executable='robot_state_publisher', 48 | name='robot_state_publisher', 49 | output='screen', 50 | parameters=[ 51 | { 52 | 'robot_description': urdf_contents, 53 | 'publish_frequency': 500.0, 54 | 'use_sim_time': use_sim_time 55 | } 56 | ] 57 | ) 58 | 59 | # rviz2 60 | rviz2_node = Node( 61 | package='rviz2', 62 | executable='rviz2', 63 | name='rviz2', 64 | output='screen', 65 | arguments=['-d', [os.path.join(visual_share, 'rviz', 'cyberdog_visual_replay.rviz')]] 66 | ) 67 | 68 | return [joint_state_node, robot_state_node, rviz2_node] 69 | 70 | def generate_launch_description(): 71 | return launch.LaunchDescription([ 72 | DeclareLaunchArgument( 73 | name='use_sim_time', 74 | default_value='false' 75 | ), 76 | DeclareLaunchArgument( 77 | name='hang_robot', 78 | default_value='false' 79 | ), 80 | DeclareLaunchArgument( 81 | name='use_lidar', 82 | default_value='false' 83 | ), 84 | DeclareLaunchArgument( 85 | name='rname', 86 | default_value='cyberdog' 87 | ), 88 | OpaqueFunction(function=launch_setup) 89 | ]) 90 | -------------------------------------------------------------------------------- /cyberdog_visual/launch/cyberdog_visual.launch.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2023-2023 Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved. 2 | 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | import os 16 | import launch 17 | import xacro 18 | from launch.actions import DeclareLaunchArgument 19 | from launch.actions import OpaqueFunction 20 | from launch.substitutions import Command, LaunchConfiguration, PythonExpression 21 | from launch_ros.actions import Node 22 | from launch_ros.substitutions import FindPackageShare 23 | 24 | def launch_setup(context, *args, **kwargs): 25 | # config 26 | hang_robot = LaunchConfiguration('hang_robot').perform(context) 27 | use_lidar = LaunchConfiguration('use_lidar').perform(context) 28 | rname = LaunchConfiguration('rname').perform(context) 29 | use_sim_time=LaunchConfiguration('use_sim_time') 30 | 31 | # path 32 | description_share = FindPackageShare( 33 | package=rname+'_description').find(rname+'_description') 34 | visual_share = FindPackageShare( 35 | package='cyberdog_visual').find('cyberdog_visual') 36 | 37 | # urdf 38 | xacro_path = os.path.join(description_share, 'xacro/robot.xacro') 39 | urdf_contents = xacro.process_file(xacro_path, mappings={ 40 | 'DEBUG': hang_robot, 'USE_LIDAR': use_lidar}).toprettyxml(indent=' ') 41 | 42 | # joint_state_publisher 43 | joint_state_node = Node( 44 | package='cyberdog_visual', 45 | executable='cyberdog_visual', 46 | name='cyberdog_visual', 47 | output='screen', 48 | parameters=[{ 49 | 'robot_description': urdf_contents, 50 | 'publish_frequency': 500.0, 51 | 'joint_state_topic': 'joint_states', 52 | 'use_sim_time': use_sim_time 53 | } 54 | ] 55 | ) 56 | 57 | # robot_state_publisher 58 | robot_state_node = Node( 59 | package='robot_state_publisher', 60 | executable='robot_state_publisher', 61 | name='robot_state_publisher', 62 | output='screen', 63 | parameters=[ 64 | { 65 | 'robot_description': urdf_contents, 66 | 'publish_frequency': 500.0, 67 | 'use_sim_time': use_sim_time 68 | } 69 | ] 70 | ) 71 | 72 | # rviz2 73 | rviz2_node = Node( 74 | package='rviz2', 75 | executable='rviz2', 76 | name='rviz2', 77 | output='screen', 78 | arguments=['-d', [os.path.join(visual_share, 'rviz', 'cyberdog_visual2.rviz')]] 79 | ) 80 | 81 | return [joint_state_node, robot_state_node, rviz2_node] 82 | 83 | def generate_launch_description(): 84 | return launch.LaunchDescription([ 85 | DeclareLaunchArgument( 86 | name='use_sim_time', 87 | default_value='true' 88 | ), 89 | DeclareLaunchArgument( 90 | name='hang_robot', 91 | default_value='false' 92 | ), 93 | DeclareLaunchArgument( 94 | name='use_lidar', 95 | default_value='false' 96 | ), 97 | DeclareLaunchArgument( 98 | name='rname', 99 | default_value='cyberdog' 100 | ), 101 | OpaqueFunction(function=launch_setup) 102 | ]) 103 | -------------------------------------------------------------------------------- /cyberdog_visual/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | cyberdog_visual 5 | 0.0.0 6 | The cyberdog virsual package 7 | You Yangwei 8 | Apache License, Version 2.0 9 | ament_cmake 10 | 11 | rclcpp 12 | tf2 13 | tf2_eigen 14 | tf2_ros 15 | tf2_geometry_msgs 16 | sensor_msgs 17 | gazebo_msgs 18 | urdf 19 | 20 | 21 | ament_cmake 22 | 23 | 24 | -------------------------------------------------------------------------------- /cyberdog_visual/src/cyberdog_visual_node.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023-2023 Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "cyberdog_visual/cyberdog_visual.hpp" 16 | int main(int argc, char** argv) { 17 | rclcpp::init(argc, argv); 18 | auto node = std::make_shared(); 19 | rclcpp::executors::MultiThreadedExecutor exec_; 20 | exec_.add_node(node->get_node_base_interface()); 21 | exec_.spin(); 22 | rclcpp::shutdown(); 23 | return 0; 24 | } --------------------------------------------------------------------------------