diff --git a/CMakeLists.txt b/CMakeLists.txt index 62eb83c..a9f4b81 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,19 +1,15 @@ cmake_minimum_required(VERSION 3.0) -project(z1_controller) +project(z1_controller LANGUAGES CXX) add_definitions(-std=c++14) +set(PACKAGE_NAME z1_controller) set(CMAKE_CXX_FLAGS "$ENV{CXXFLAGS} -O3") -# set(GRIPPER MASS3KG) # 3kg -# set(GRIPPER NONE) # No Gripper -set(GRIPPER UNITREE_GRIPPER) # The gripper made by unitree - -set(COMMUNICATION UDP) # UDP -# set(COMMUNICATION ROS) # ROS - -# set(CTRL_PANEL KEYBOARD) # control by keyboard -set(CTRL_PANEL SDK) # control by SDK -# set(CTRL_PANEL JOYSTICK) # control by joystick +# ----------------------options---------------------- +set(COMMUNICATION UDP) # UDP +# set(COMMUNICATION ROS) # ROS +# ----------------------configurations---------------------- +# COMMUNICATION if(${COMMUNICATION} STREQUAL "UDP") add_definitions(-DUDP) set(SIMULATION OFF) @@ -26,6 +22,7 @@ else() message(FATAL_ERROR "[CMake ERROR] The COMMUNICATION is error") endif() +# SIM OR REAL if(((SIMULATION) AND (REAL_ROBOT)) OR ((NOT SIMULATION) AND (NOT REAL_ROBOT))) message(FATAL_ERROR "[CMake ERROR] The SIMULATION and REAL_ROBOT can only be one ON one OFF") endif() @@ -40,34 +37,8 @@ if(SIMULATION) find_package(gazebo REQUIRED) endif() -if(DEBUG) - add_definitions(-DCOMPILE_DEBUG) - find_package(Python2 COMPONENTS Interpreter Development NumPy) -endif() - - -if(${GRIPPER} STREQUAL "NONE") - add_definitions(-DNO_GRIPPER) -elseif(${GRIPPER} STREQUAL "AG95") - add_definitions(-DAG95) -elseif(${GRIPPER} STREQUAL "MASS3KG") - add_definitions(-DMASS3KG) -elseif(${GRIPPER} STREQUAL "UNITREE_GRIPPER") - add_definitions(-DUNITREE_GRIPPER) -else() - message(FATAL_ERROR "[CMake ERROR] The GRIPPER is error") -endif() - - -if(${CTRL_PANEL} STREQUAL "KEYBOARD") - add_definitions(-DCTRL_BY_KEYBOARD) -elseif(${CTRL_PANEL} STREQUAL "JOYSTICK") - add_definitions(-DCTRL_BY_JOYSTICK) -elseif(${CTRL_PANEL} STREQUAL "SDK") - add_definitions(-DCTRL_BY_SDK) -else() - message(FATAL_ERROR "[CMake ERROR] The CTRL_PANEL is error") -endif() +# ARM_MODEL +add_definitions(-DARM_Z1) if(SIMULATION) find_package(catkin REQUIRED COMPONENTS @@ -110,35 +81,9 @@ include_directories( link_directories(lib) -# # add_library -# file(GLOB_RECURSE ADD_SRC_LIST -# src/*/*.cpp -# src/*/*/*.cpp -# thirdparty/*/src/*.cpp -# thirdparty/*/src/*.cc -# ../z1_sdk/src/*.cpp -# ) - -# set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/../lib) # SHARED:CMAKE_LIBRARY_OUTPUT_DIRECTORY STATIC:CMAKE_ARCHIVE_OUTPUT_DIRECTORY -# add_library(Z1_${CTRL_PANEL}_${COMMUNICATION}_Linux64 SHARED ${ADD_SRC_LIST}) -# target_link_libraries(Z1_${CTRL_PANEL}_${COMMUNICATION}_Linux64 ${catkin_LIBRARIES} rbdl -pthread) -# if(SIMULATION) -# add_dependencies(Z1_${CTRL_PANEL}_${COMMUNICATION}_Linux64 ${${PEOJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -# endif() - - -# add_executable and target_link_libraries +# ----------------------add executable---------------------- set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/../build) # EXECUTABLE:CMAKE_RUNTIME_OUTPUT_DIRECTORY add_executable(z1_ctrl main.cpp) -target_link_libraries(z1_ctrl ${catkin_LIBRARIES} libUnitree_motor_SDK_Linux64_EPOLL.so libZ1_${CTRL_PANEL}_${COMMUNICATION}_Linux64.so) +target_link_libraries(z1_ctrl ${catkin_LIBRARIES} libUnitree_motor_SDK_Linux64_EPOLL.so libZ1_${COMMUNICATION}_Linux64.so) - -# debug -if(DEBUG) - find_package(PythonLibs REQUIRED) - include_directories(${PYTHON_INCLUDE_DIRS}) - target_link_libraries(Z1_${CTRL_PANEL}_${COMMUNICATION}_Linux64 ${PYTHON_LIBRARIES}) -endif() - - -# set(EXECUTABLE_OUTPUT_PATH /home/$ENV{USER}/) \ No newline at end of file +# set(EXECUTABLE_OUTPUT_PATH /home/$ENV{USER}/) diff --git a/README.md b/README.md index 4e14b59..55e1d8f 100644 --- a/README.md +++ b/README.md @@ -1,223 +1 @@ -# v3.6.1 -The z1_controller is mainly used to control the robot arm and communicate with the Z1 SDK - -## Notice -support robot: Z1(v3.6.1) -not support robot: Z1(v3.4, v3.5) - -## Dependencies -- [ROS](https://www.ros.org/)（Melodic） -- build-essential - -```bash -sudo apt install build-essential -``` - -- Boost (version 1.5.4 or higher) - -```bash -dpkg -S /usr/include/boost/version.hpp # check boost version -sudo apt install libboost-dev # install boost -``` - -- CMake (version 2.8.3 or higher) - -```bash -cmake --version # check cmake version -sudo apt install cmake # install cmake -``` - -- [Eigen](https://gitlab.com/libeigen/eigen/-/releases/3.3.9) (version 3.3.9 or higher) - -```bash -cd eigen-3.3.9 -mkdir build && cd build -cmake .. -sudo make install -sudo ln -s /usr/local/include/eigen3 /usr/include/eigen3 -sudo ln -s /usr/local/include/eigen3/Eigen /usr/local/include/Eigen -``` - -- [RBDL](https://github.com/rbdl/rbdl/releases/tag/v2.6.0) (version 2.6.0) - -```bash -cd rbdl-2.6.0 -mkdir build && cd build -cmake -D CMAKE_BUILD_TYPE=Release .. -sudo make install -sudo sh -c "echo '/usr/local/lib' >> /etc/ld.so.conf" -sudo ldconfig -``` - -## Build - -```bash -# config //z1_controller/CMakeList.txt -cd //z1_controller && mkdir build && cd build -cmake .. -make -j4 -``` - -## Usage - -The default IP of the robot is 192.168.123.110, you need to change the IP of your PC before using the SDK so that your PC can ping to the robot. - -First, please connect the network cable between your PC and robot. Then run ifconfig in a terminal, you will find your port name. For example, enpxxx. - -```bash -sudo ifconfig enpxxx down # enpxxx is your PC port -sudo ifconfig enpxxx 192.168.123.162/24 -sudo ifconfig enpxxx up -ping 192.168.123.110 -``` - -Then run program. - -``` -cd //z1_controller/build -sudo ./z1_ctrl -``` - -- FSM(finite-state machine) - -| State | Keyswitch | Switchable | -| ----------- | --------- | --------------------------- | -| BACKTOSTART | ～ | 1 2 | -| PASSIVE | 1 | ~ 2 3 = | -| JOINTCTRL | 2 | ~ 1 3 4 5 6 7 8 9 0 - | -| CARTESIAN | 3 | ~ 1 2 4 5 6 9 | -| MoveJ | 4 | ~ 1 2 3 5 6 9 | -| MoveL | 5 | ~ 1 2 3 4 6 9 | -| MOVEC | 6 | ~ 1 2 3 4 5 9 | -| TEACH | 7 | ~ 1 2 | -| TEACHREPEAT | 8 | automatically switches to 2 | -| SAVESTATE | 9 | automatically switches to 2 | -| TOSTATE | 0 | automatically switches to 2 | -| TRAJECTORY | - | ~ 1 2 | -| CALIBRATION | = | automatically switches to 2 | -| NEXT | ] | next state | - -#### FSM Details - -- Key ~ (BACKTOSTART) : All motors return to initial positions - -- Key1（PASSIVE）: All motor enter passive state(The state of the Z1 startup) - -- Key2（ JOINTCTRL） - - | Joint ID | 0 | 1 | 2 | 3 | 4 | 5 | Gripper | - | :------------------------------ | :---------------------- | :---------------------- | :---------------------- | :---------------------- | :---------------------- | :---------------------- | ----------------------- | - | Keyboard | Q/A | W/ S | D/ E | R/F | T/G | Y/H | up/down | - | Joint Action
(right hand) | positive
/negative | positive
/negative | positive
/negative | positive
/negative | positive
/negative | positive
/negative | positive
/negative | - -- Key3（CARTESIAN）: The reference coordinate system is cartesian - - | Keyboard | Q/A | W/S | E/D | R/F | T/G | Y/H | - | ------------ | ---------------------- | ---------------- | ------------- | ----------------------- | ------------------------ | ---------------------- | - | Key Function | forward
/backward | right
/left | up
/down | roll
(right hand) | pitch
(right hand) | yaw
(right hand) | - -- Key4（MoveJ）: - - ``` - Key4—— Enter the desired end pose（roll pitch yaw x y z）——The Z1 joint rotate to the joint target point（After Z1 arrived the target point, it automatically switches to the joint space control state） - ``` - -- Key5（MoveL）: - - ``` - Key5——Enter the desired end pose（roll pitch yaw x y z）——The Z1 follows the generated straight trajectory to the target point（After Z1 arrived the target point, it automatically switches to the joint space control state） - ``` - -- Key6（MoveC）: - - ``` - Key6——Enter the desired middle and end pose（roll pitch yaw x y z）——The Z1 follows the generated arc trajectory to the target point（After Z1 arrived the target point, it automatically switches to the joint space control state） - ``` - -- Key7（TEACH）: - - ``` - Key7——Enter the teaching trajectory label —— Drag Z1 —— Press Key2 to complete teaching. - ``` - -- Key8（TEACHREPEAT）: - - ``` - Key8———— Enter the saved teaching trajectory label—— Z1 repeate the teaching trajectory - ``` - -- Key9（SAVESTATE）: - - ``` - Key9——Enter the current pose label —— Z1 automatically switches to the joint space control state - ``` - -- Key0（TOSTATE）: - - ``` - Key0——Enter the pose label to save（After Z1 arrived the target point, it automatically switches to the joint space control state） - ``` - -- Key -（TRAJECTORY）: - - ``` - Key - —— Z1 repeats in a written trajectory - ``` - -- Key=（CALIBRATION)； - - ``` - Key= —— Set the current position as the initial position, and enter the joint space control state after setting - ``` - -- Key ] （NEXT）: used to debug joystick contrl - - ``` - Key]—— Enter next state - ``` - -## SDK - -If you want to develop your own control methods of Z1, you can refer to the SDK(z1_sdk). - -We have written an example of keyboard control based on SDK, you can use it fllowing the steps below. - -### State change - -- First, set(CTRL_PANEL SDK) # z1_ws/src/z1_controller/CMakeList.txt，and then rebuild the z1_ws to generate z1_ctrl, then open a teminal to run z1_ctrl - - ``` - cd //z1_controller/build - sudo ./z1_ctrl - ``` - -- Sencond, build the z1_sdk, and then open another terminal to run example. - - ``` - cd //z1_sdk && mkdir build && cd build - cmake .. - make -j4 - ./example_state_send - ``` - -### Low level control - -``` -sudo ./z1_ctrl # Running in a terminal -./example_lowCmd_send # Running in another terminal -``` - -### Keyboard control - -``` -sudo ./z1_ctrl # Running in a terminal -./example_keyboard_send # Running in another terminal -``` - -### State control - -``` -sudo ./z1_ctrl # Running in a terminal -./example_keyboard_send # Running in another terminal -``` - +see [unitree-z1-docs](http://dev-z1.unitree.com/) \ No newline at end of file diff --git a/config.xml b/config.xml new file mode 100644 index 0000000..5208b07 --- /dev/null +++ b/config.xml @@ -0,0 +1,26 @@ + + + keyboard + sdk + joystick + + + + + + + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + + + + 192.168.123.110 + diff --git a/config/savedArmStates.csv b/config/savedArmStates.csv index 63de734..0201ef6 100755 --- a/config/savedArmStates.csv +++ b/config/savedArmStates.csv @@ -1,59 +1,14 @@ -forward, -0.000019, 1.542859, -1.037883, -0.531308, 0.002487, -0.012173, 0.999650, -0.002146, -0.026357, 0.424746, 0.002468, 0.999923, 0.012173, 0.000358, 0.026329, -0.012234, 0.999578, 0.394252, 0.000000, 0.000000, 0.000000, 1.000000, -start, -0.000336, 0.001634, 0.008171, 0.064640, 0.000248, 0.000230, 0.997230, 0.000106, 0.074376, 0.085128, -0.000087, 1.000000, -0.000255, 0.000008, -0.074376, 0.000248, 0.997230, 0.133277, 0.000000, 0.000000, 0.000000, 1.000000, -highRiseLeft, -0.756000, 0.500000, -1.496000, -0.942000, -0.879000, -1.004000, 0.403000, 0.633000, 0.661000, -0.102000, 0.476000, -0.762000, 0.440000, 0.178000, 0.782000, 0.137000, -0.608000, 0.610000, 0.000000, 0.000000, 0.000000, 1.000000, -lowPickLeft, 0.590000, 2.088000, -0.899000, 0.591000, 0.906000, -0.641000, -0.558000, -0.521000, 0.646000, 0.221000, 0.155000, 0.699000, 0.698000, 0.198000, -0.816000, 0.489000, -0.309000, 0.071000, 0.000000, 0.000000, 0.000000, 1.000000, +forward, -0.000019, 1.542859, -1.037883, -0.531308, 0.002487, -0.012173, 0.999650, -0.002146, -0.026357, 0.389527, 0.002468, 0.999923, 0.012173, 0.000269, 0.026329, -0.012234, 0.999578, 0.402549, 0.000000, 0.000000, 0.000000, 1.000000, +start, -0.000336, 0.001634, 0.000000, 0.064640, 0.000248, 0.000230, 0.997805, 0.000104, 0.066225, 0.048696, -0.000087, 1.000000, -0.000252, 0.000011, -0.066225, 0.000246, 0.997805, 0.148729, 0.000000, 0.000000, 0.000000, 1.000000, startFlat, 0.000000, 0.000000, -0.000012, -0.074351, 0.000000, 0.000006, 0.997236, -0.000000, -0.074294, 0.086594, 0.000000, 1.000000, -0.000006, 0.000000, 0.074294, 0.000006, 0.997236, 0.176788, 0.000000, 0.000000, 0.000000, 1.000000, -videoHighRise, 0.804000, 0.500000, -1.496000, -0.942000, -0.879000, -1.004000, -0.471646, 0.768269, -0.432796, -0.179591, 0.408109, 0.625271, 0.665195, -0.100048, 0.781663, 0.137109, -0.608444, 0.610103, 0.000000, 0.000000, 0.000000, 1.000000, -videoToLeft, 2.316499, 2.223266, -1.131086, 0.590578, 0.905720, -0.640975, -0.121092, -0.577184, -0.807586, -0.278129, -0.514682, -0.659161, 0.548278, 0.239010, -0.848787, 0.482042, -0.217247, 0.058040, 0.000000, 0.000000, 0.000000, 1.000000, -video_02, -0.042000, 1.416000, -1.484000, 0.000000, -0.279000, 0.000000, 0.939568, 0.041988, -0.339778, 0.322181, -0.039485, 0.999118, 0.014279, -0.013540, 0.340078, 0.000000, 0.940397, 0.545628, 0.000000, 0.000000, 0.000000, 1.000000, -video_03, 0.918960, 2.171685, -1.471256, 0.000000, 0.427000, 0.000000, 0.260242, -0.794971, 0.547992, 0.300209, 0.341030, 0.606647, 0.718107, 0.393404, -0.903312, 0.000000, 0.428984, 0.149885, 0.000000, 0.000000, 0.000000, 1.000000, -video_01, -0.871858, 0.870714, -0.722365, 0.055117, -0.678396, -0.070888, 0.528563, 0.793043, -0.302826, 0.095419, -0.682618, 0.609129, 0.403725, -0.118699, 0.504632, -0.006679, 0.863309, 0.420252, 0.000000, 0.000000, 0.000000, 1.000000, -test, 0.368525, 2.393267, -1.148771, -0.953744, 0.368296, 0.368360, 0.704081, -0.517416, 0.486365, 0.522431, 0.657831, 0.733192, -0.172300, 0.259328, -0.267449, 0.441259, 0.856599, 0.093448, 0.000000, 0.000000, 0.000000, 1.000000, -repeatLow, 0.000000, 1.931603, -0.835792, 0.000000, 0.390233, 0.000000, 0.084651, 0.000000, 0.996411, 0.311108, 0.000000, 1.000000, 0.000000, 0.000000, -0.996411, 0.000000, 0.084651, 0.092545, 0.000000, 0.000000, 0.000000, 1.000000, -repeatHigh, 0.000000, 1.940186, -1.486322, 0.000000, 0.391440, 0.000000, 0.663504, 0.000000, 0.748173, 0.474034, 0.000000, 1.000000, 0.000000, 0.000000, -0.748173, 0.000000, 0.663504, 0.269367, 0.000000, 0.000000, 0.000000, 1.000000, -winestart, -0.550922, 2.147376, -0.743038, -0.055447, -1.430589, 1.647373, 0.879196, -0.054860, -0.473292, 0.332236, -0.475751, -0.046881, -0.878330, -0.197922, 0.025997, 0.997393, -0.067317, 0.113427, 0.000000, 0.000000, 0.000000, 1.000000, -wineget, -0.609784, 2.218792, -0.937567, -0.117771, -1.309136, 1.671756, 0.879196, -0.054860, -0.473292, 0.366936, -0.475751, -0.046881, -0.878330, -0.243022, 0.025997, 0.997393, -0.067317, 0.113427, 0.000000, 0.000000, 0.000000, 1.000000, -wine1, -0.346864, 1.931170, -0.980517, -0.398801, -1.091111, 0.639763, 0.995012, -0.045873, -0.088587, 0.410574, 0.006629, 0.916446, -0.400104, -0.113175, 0.099539, 0.397521, 0.912178, 0.226441, 0.000000, 0.000000, 0.000000, 1.000000, -wine2, -0.346864, 1.931170, -0.980517, -0.398801, -1.091111, 0.319763, 0.995012, -0.015678, -0.098520, 0.410574, 0.006629, 0.995782, -0.091510, -0.113175, 0.099539, 0.090400, 0.990919, 0.226441, 0.000000, 0.000000, 0.000000, 1.000000, -wine3, -0.346864, 1.931170, -0.980517, -0.398801, -1.091111, 1.829763, 0.995012, -0.099290, 0.009662, 0.410574, 0.006629, -0.030838, -0.999502, -0.113175, 0.099539, 0.994581, -0.030026, 0.226441, 0.000000, 0.000000, 0.000000, 1.000000, -wine4, -0.609784, 2.103729, -0.954345, -0.123154, -1.178247, 1.688475, 0.879196, -0.054860, -0.473292, 0.366936, -0.475751, -0.046881, -0.878330, -0.243022, 0.025997, 0.997393, -0.067317, 0.155627, 0.000000, 0.000000, 0.000000, 1.000000, -wine2_5, -0.270810, 1.913207, -0.952264, -0.313249, -1.085456, 0.511294, 0.995012, -0.038831, -0.091892, 0.410574, 0.006629, 0.944836, -0.327477, -0.086775, 0.099539, 0.325234, 0.940380, 0.226441, 0.000000, 0.000000, 0.000000, 1.000000, -doorstart, 0.431042, 1.563797, -1.529086, 1.322867, -0.534746, -1.320230, 0.991589, 0.086371, -0.096392, 0.358317, -0.087764, 0.996088, -0.010297, 0.112472, 0.095125, 0.018670, 0.995290, 0.496540, 0.000000, 0.000000, 0.000000, 1.000000, -doorstartflat, 0.520059, 1.879836, -1.920901, 1.443620, -0.682288, -1.471548, 0.980922, 0.157107, -0.114496, 0.436491, -0.159056, 0.987237, -0.008031, 0.180614, 0.111773, 0.026089, 0.993391, 0.503541, 0.000000, 0.000000, 0.000000, 1.000000, -doorpre, 0.431042, 1.475972, -1.314848, 1.466425, -0.591235, -1.494022, 0.984623, 0.149591, 0.090224, 0.332506, -0.157354, 0.983780, 0.086116, 0.094216, -0.075878, -0.098989, 0.992191, 0.441392, 0.000000, 0.000000, 0.000000, 1.000000, -doorstartflat2, 1.085467, 1.374296, -1.381275, 1.454993, -1.117510, -1.514257, 0.993818, 0.028773, -0.107233, 0.198932, -0.029880, 0.999515, -0.008726, 0.193041, 0.106930, 0.011876, 0.994196, 0.503075, 0.000000, 0.000000, 0.000000, 1.000000, -doorpre2, 1.277563, 1.233555, -1.023053, 1.407057, -1.338097, -1.314977, 0.993818, 0.028774, -0.107232, 0.147332, -0.029881, 0.999515, -0.008725, 0.168441, 0.106929, 0.011875, 0.994196, 0.426275, 0.000000, 0.000000, 0.000000, 1.000000, -doorstart2, 0.894370, 1.364286, -1.338585, 1.417931, -0.932419, -1.446631, 0.993818, 0.028774, -0.107232, 0.233132, -0.029881, 0.999515, -0.008725, 0.168441, 0.106929, 0.011875, 0.994196, 0.492875, 0.000000, 0.000000, 0.000000, 1.000000, -swardhit01, 0.046000, 1.293000, -1.235000, 0.000000, -0.616000, 0.000000, 0.847418, -0.045984, -0.528931, 0.278086, 0.039009, 0.998942, -0.024348, 0.012801, 0.529491, 0.000000, 0.848316, 0.518108, 0.000000, 0.000000, 0.000000, 1.000000, -swardmiddle01, 0.824000, 1.151000, -0.931000, 0.000000, -0.604000, 0.000000, 0.629821, -0.733869, -0.254484, 0.163849, 0.680424, 0.679291, -0.274931, 0.177013, 0.374632, 0.000000, 0.927174, 0.438554, 0.000000, 0.000000, 0.000000, 1.000000, -swardstart01, 0.000000, 0.861000, -0.455000, 0.000000, -0.518000, 0.000000, 0.993735, 0.000000, -0.111766, 0.154853, 0.000000, 1.000000, 0.000000, 0.000000, 0.111766, 0.000000, 0.993735, 0.304846, 0.000000, 0.000000, 0.000000, 1.000000, -swardpush01, -0.246000, 1.701000, -1.361000, 1.042000, -0.442000, 0.000000, 0.806358, 0.402127, -0.433683, 0.403978, -0.583253, 0.419186, -0.695773, -0.138065, -0.097995, 0.813988, 0.572555, 0.385327, 0.000000, 0.000000, 0.000000, 1.000000, -swardhit02, -0.060000, 2.293000, -2.333000, 0.056000, -0.052000, 0.000000, 0.993808, 0.057636, -0.094994, 0.613902, -0.062615, 0.996770, -0.050289, -0.037159, 0.091789, 0.055926, 0.994207, 0.430516, 0.000000, 0.000000, 0.000000, 1.000000, -swardthroat, 0.018000, 1.842000, -2.130000, -1.372000, -1.373000, 1.128000, 0.116093, -0.744644, -0.657288, 0.368208, 0.963437, 0.245315, -0.107751, 0.099110, 0.241479, -0.620746, 0.745897, 0.588202, 0.000000, 0.000000, 0.000000, 1.000000, -swardswing, 0.000000, 1.947000, -0.899000, -1.022000, -0.948000, 0.000000, 0.658370, -0.739194, -0.141916, 0.382308, 0.692974, 0.521661, 0.497652, 0.066664, -0.293829, -0.425983, 0.855688, 0.167033, 0.000000, 0.000000, 0.000000, 1.000000, -swardstart, 0.000000, 1.695335, -1.003223, -0.000000, -0.980112, 0.000000, 0.958814, 0.000000, -0.284035, 0.392642, 0.000000, 1.000000, 0.000000, 0.000000, 0.284035, 0.000000, 0.958814, 0.324838, 0.000000, 0.000000, 0.000000, 1.000000, -swardmiddle02, -0.009336, 1.762476, -0.547022, 0.071902, -1.570413, -0.026650, 0.934527, 0.085894, -0.345372, 0.306754, -0.080568, 0.996304, 0.029776, -0.009775, 0.346653, -0.000001, 0.937993, 0.218133, 0.000000, 0.000000, 0.000000, 1.000000, -swardend, -0.009336, 0.640476, -0.385022, 0.071902, -0.550413, -0.026650, 0.956080, 0.035230, -0.290981, 0.105389, -0.046503, 0.998408, -0.031915, -0.004599, 0.289393, 0.044045, 0.956196, 0.309459, 0.000000, 0.000000, 0.000000, 1.000000, -swardwalk, 0.000000, 0.861000, -0.455000, 0.000000, -1.068000, 0.000000, 0.788764, 0.000000, -0.614696, 0.135135, 0.000000, 1.000000, 0.000000, 0.000000, 0.614696, 0.000000, 0.788764, 0.353228, 0.000000, 0.000000, 0.000000, 1.000000, -nokiaend, -0.106199, 2.620994, -1.475180, 0.022797, -0.839897, -1.203857, 0.946225, -0.232455, 0.224997, 0.559486, -0.117937, 0.399763, 0.908999, -0.061283, -0.301247, -0.886653, 0.350851, -0.002630, 0.000000, 0.000000, 0.000000, 1.000000, -nokiamiddle, -0.136319, 2.083721, -1.059621, -0.017853, -0.718157, -1.184266, 0.946225, -0.232455, 0.224996, 0.455086, -0.117937, 0.399764, 0.908999, -0.061283, -0.301247, -0.886653, 0.350851, 0.150371, 0.000000, 0.000000, 0.000000, 1.000000, -nokiastart, 0.000000, 0.861000, -0.455000, 0.000000, -1.068000, 0.000000, 0.788764, 0.000000, -0.614696, 0.135135, 0.000000, 1.000000, 0.000000, 0.000000, 0.614696, 0.000000, 0.788764, 0.353228, 0.000000, 0.000000, 0.000000, 1.000000, teachstart, -0.858000, 1.951000, -0.825000, 0.000000, -1.154000, 0.000000, 0.653696, 0.756536, -0.018308, 0.260474, -0.756240, 0.653952, 0.021180, -0.301334, 0.027996, 0.000000, 0.999608, 0.183326, 0.000000, 0.000000, 0.000000, 1.000000, -djistart, 0.000000, 1.616000, -0.324000, 0.000000, -1.285000, 0.000000, 0.999976, 0.000000, 0.007000, 0.242349, 0.000000, 1.000000, 0.000000, 0.000000, -0.007000, 0.000000, 0.999976, 0.179422, 0.000000, 0.000000, 0.000000, 1.000000, -anyway, 0.242441, 0.267866, -0.016100, -0.008059, -0.122301, 0.005914, 0.962394, -0.241280, 0.124828, 0.050463, 0.239018, 0.970454, 0.033019, 0.012577, -0.129107, -0.001941, 0.991629, 0.153768, 0.000000, 0.000000, 0.000000, 1.000000, fulltest00, 2.316194, 0.013239, -0.001959, -0.000047, -0.278982, 0.221496, -0.654115, -0.677458, 0.336427, -0.022776, 0.708636, -0.704403, -0.040645, 0.024673, 0.264516, 0.211818, 0.940832, 0.174303, 0.000000, 0.000000, 0.000000, 1.000000, fulltest01, 0.000194, 1.621239, -1.455959, 2.470727, 1.492796, 0.419496, 0.205219, 0.489994, 0.847225, 0.332273, 0.619814, -0.735004, 0.274956, 0.059687, 0.757440, 0.468696, -0.454542, 0.521459, 0.000000, 0.000000, 0.000000, 1.000000, fulltest02, -2.336194, 3.117239, -3.099593, -2.478727, -1.516796, 0.419496, 0.415340, -0.220546, 0.882526, -0.404129, -0.454627, 0.789988, 0.411380, -0.505948, -0.787913, -0.572083, 0.227847, 0.075097, 0.000000, 0.000000, 0.000000, 1.000000, -5kg01, -0.582000, 2.072006, -0.858000, 0.000000, 0.241000, 0.000000, 0.096511, 0.549696, 0.829771, 0.280359, -0.063507, 0.835365, -0.546015, -0.184485, -0.993304, 0.000000, 0.115532, 0.003142, 0.000000, 0.000000, 0.000000, 1.000000, -5kg02, -0.582000, 1.854695, -0.950446, -0.000000, 0.550757, -0.000000, 0.096511, 0.549696, 0.829771, 0.280359, -0.063507, 0.835365, -0.546015, -0.184485, -0.993304, 0.000000, 0.115532, 0.090142, 0.000000, 0.000000, 0.000000, 1.000000, -5kg03, 0.524000, 1.854695, -0.950446, -0.000000, 0.550757, -0.000000, 0.100030, -0.500347, 0.860027, 0.290582, 0.057806, 0.865825, 0.496997, 0.167923, -0.993304, 0.000000, 0.115532, 0.090142, 0.000000, 0.000000, 0.000000, 1.000000, -5kg04, 0.524000, 2.008825, -0.874538, 0.000000, 0.320718, -0.000000, 0.100030, -0.500347, 0.860027, 0.290582, 0.057806, 0.865825, 0.496997, 0.167923, -0.993304, 0.000000, 0.115532, 0.025942, 0.000000, 0.000000, 0.000000, 1.000000, -hold_light, -0.000024, 1.098000, -1.618000, 0.000000, 0.340994, 0.000000, 0.984021, 0.000024, -0.178051, 0.161036, -0.000023, 1.000000, 0.000004, -0.000004, 0.178051, 0.000000, 0.984021, 0.613488, 0.000000, 0.000000, 0.000000, 1.000000, -give_light, -0.000024, 2.396000, -2.156000, 0.000000, 0.340994, 0.000000, 0.835917, 0.000024, 0.548855, 0.631917, -0.000020, 1.000000, -0.000013, -0.000015, -0.548855, 0.000000, 0.835917, 0.261657, 0.000000, 0.000000, 0.000000, 1.000000, -dance_swing_right, 0.987976, 0.692000, -1.152000, 0.000000, 0.340994, 0.000000, 0.546488, -0.834914, -0.065344, 0.034540, 0.829009, 0.550381, -0.099125, 0.052396, 0.118725, 0.000000, 0.992927, 0.505625, 0.000000, 0.000000, 0.000000, 1.000000, -dance_swing_left, -0.970024, 0.692000, -1.152000, 0.000000, 0.340994, 0.000000, 0.561282, 0.824899, -0.067113, 0.035475, -0.819065, 0.565280, 0.097936, -0.051768, 0.118725, 0.000000, 0.992927, 0.505625, 0.000000, 0.000000, 0.000000, 1.000000, 001, 0.000000, 0.922204, -1.215402, 0.293198, 0.000000, 0.000000, 1.000000, 0.000000, -0.000000, 0.200000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.500000, 0.000000, 0.000000, 0.000000, 1.000000, 002, -1.035000, 0.922204, -1.017402, -0.285802, 0.000000, 0.000000, 0.473918, 0.859862, -0.189839, 0.103971, -0.798204, 0.510526, 0.319739, -0.175114, 0.371849, 0.000000, 0.928293, 0.541400, 0.000000, 0.000000, 0.000000, 1.000000, 003, 0.954000, 0.910204, -0.984402, -0.192802, 0.000000, 0.000000, 0.557929, -0.815736, -0.152611, 0.121384, 0.786832, 0.578425, -0.215223, 0.171185, 0.263839, 0.000000, 0.964567, 0.510707, 0.000000, 0.000000, 0.000000, 1.000000, -starFlat, -0.003451, 0.000501, 0.009250, -0.064964, 0.000525, 0.000590, 0.998472, 0.002895, -0.055186, 0.085395, -0.002921, 0.999996, -0.000400, -0.000217, 0.055184, 0.000560, 0.998476, 0.161033, 0.000000, 0.000000, 0.000000, 1.000000, -yy, 0.000000, 0.922204, -1.215402, 0.293198, 0.000000, 0.000000, 1.000000, 0.000000, -0.000000, 0.200000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.500000, 0.000000, 0.000000, 0.000000, 1.000000, +mohe1, 0.966000, 1.420800, -1.412412, -1.141151, 0.997200, 1.675206, -0.560087, -0.444472, 0.699105, 0.049195, 0.666938, -0.742508, 0.062250, 0.235148, 0.491423, 0.501125, 0.712304, 0.622679, 0.000000, 0.000000, 0.000000, 1.000000, +mohe2, -1.057200, 2.424000, -2.682793, 0.986449, -1.342800, -1.709994, -0.765519, -0.400523, 0.503550, 0.166634, -0.625604, 0.646204, -0.437081, -0.515483, -0.150335, -0.649616, -0.745251, 0.381296, 0.000000, 0.000000, 0.000000, 1.000000, +5kg1, 0.661051, 3.010641, -2.885593, -0.233059, 0.000065, 0.000206, 0.784707, -0.614016, -0.084965, 0.592966, 0.610421, 0.789294, -0.066347, 0.461215, 0.107801, 0.000198, 0.994173, 0.198126, 0.000000, 0.000000, 0.000000, 1.000000, +5kg2, -0.495523, 3.122645, -2.856166, -0.406859, 0.000065, -0.000378, 0.871097, 0.475482, -0.122910, 0.664711, -0.470757, 0.879726, 0.066863, -0.359274, 0.139919, -0.000383, 0.990163, 0.133116, 0.000000, 0.000000, 0.000000, 1.000000, diff --git a/include/FSM/BaseState.h b/include/FSM/BaseState.h index ea15189..b070c9b 100755 --- a/include/FSM/BaseState.h +++ b/include/FSM/BaseState.h @@ -2,6 +2,7 @@ #define BASESTATE_H #include +#include "common/enumClass.h" class BaseState{ public: @@ -25,6 +26,7 @@ public: protected: int _stateNameEnum; std::string _stateNameString; + Control _ctrl; }; #endif \ No newline at end of file diff --git a/include/FSM/FSMState.h b/include/FSM/FSMState.h index 4e825bd..920f531 100755 --- a/include/FSM/FSMState.h +++ b/include/FSM/FSMState.h @@ -24,7 +24,7 @@ public: virtual void run() = 0; virtual void exit() = 0; virtual int checkChange(int cmd) {return (int)ArmFSMStateName::INVALID;} - + protected: void _armCtrl(); void _tauDynForward(); @@ -33,20 +33,21 @@ protected: bool _collisionTest(); void _jointPosProtect(); void _jointSpeedProtect(); -#ifdef UNITREE_GRIPPER + void _stateUpdate(); + void _gripperCmd(); void _gripperCtrl(); double _gripperPos; double _gripperW; double _gripperTau; - double _gripperPosStep; + double _gripperPosStep;//keyboard double _gripperTauStep; double _gripperLinearFriction; double _gripperCoulombFriction; static double _gripperCoulombDirection; double _gripperFriction; -#endif + CtrlComponents *_ctrlComp; ArmFSMStateName _nextStateName; diff --git a/include/FSM/FiniteStateMachine.h b/include/FSM/FiniteStateMachine.h index 5af327a..71fdbe8 100755 --- a/include/FSM/FiniteStateMachine.h +++ b/include/FSM/FiniteStateMachine.h @@ -5,6 +5,7 @@ #include "FSM/BaseState.h" #include "unitree_arm_sdk/cmdPanel.h" #include "unitree_arm_sdk/loop.h" +#include "unitree_arm_sdk/keyboard.h" enum class FSMRunMode{ NORMAL, @@ -25,7 +26,6 @@ public: private: void _run(); - static void* _staticRun(void* obj); std::vector _states; FSMRunMode _mode; diff --git a/include/FSM/State_BackToStart.h b/include/FSM/State_BackToStart.h index 1107373..a77434a 100644 --- a/include/FSM/State_BackToStart.h +++ b/include/FSM/State_BackToStart.h @@ -8,6 +8,7 @@ public: State_BackToStart(CtrlComponents *ctrlComp); private: void _setTraj(); + void _setTrajSDK(){} }; #endif // STATE_BACKTOSTART_H \ No newline at end of file diff --git a/include/FSM/State_Cartesian.h b/include/FSM/State_Cartesian.h index b7bfd66..43ac293 100755 --- a/include/FSM/State_Cartesian.h +++ b/include/FSM/State_Cartesian.h @@ -12,13 +12,13 @@ public: void exit(); int checkChange(int cmd); private: - void quadprogArea(); double _posSpeed; double _oriSpeed; Vec3 _omega; Vec3 _velocity; Vec6 _twist; HomoMat _endHomoGoal, _endHomoGoalPast; + Vec6 _endPostureGoal, _endPosturePast, _endPostureDelta; HomoMat _endHomoFeedback; Vec6 _Pdes; Vec6 _Pfd; diff --git a/include/FSM/State_Dance.h b/include/FSM/State_Dance.h index 3654443..1ef5039 100644 --- a/include/FSM/State_Dance.h +++ b/include/FSM/State_Dance.h @@ -14,7 +14,6 @@ public: int checkChange(int cmd); private: bool _freeBottom = false; - // ArmFSMStateName _nextState; void _setTraj(){} }; diff --git a/include/FSM/State_MoveL.h b/include/FSM/State_MoveL.h index b7f0f21..6bde0e4 100755 --- a/include/FSM/State_MoveL.h +++ b/include/FSM/State_MoveL.h @@ -31,5 +31,4 @@ private: CartesionSpaceTraj *_cartesionTraj; bool _timeReached, _taskReached, _pastTaskReached, _finalReached; }; - #endif // CARTESIAN_H \ No newline at end of file diff --git a/include/FSM/State_SetTraj.h b/include/FSM/State_SetTraj.h new file mode 100644 index 0000000..2767232 --- /dev/null +++ b/include/FSM/State_SetTraj.h @@ -0,0 +1,20 @@ +#ifndef STATE_SETTRAJ_H +#define STATE_SETTRAJ_H + +#include "FSM/State_Trajectory.h" + +class State_SetTraj: public State_Trajectory{ +public: + State_SetTraj(CtrlComponents *ctrlComp, + ArmFSMStateName stateEnum = ArmFSMStateName::SETTRAJ, + std::string stateString = "setTraj"); + ~State_SetTraj(); + void enter(); + void run(); + void exit(); + int checkChange(int cmd); +private: + Vec6 _posture[2]; +}; + +#endif \ No newline at end of file diff --git a/include/FSM/State_TeachRepeat.h b/include/FSM/State_TeachRepeat.h index 85d20fe..4961889 100755 --- a/include/FSM/State_TeachRepeat.h +++ b/include/FSM/State_TeachRepeat.h @@ -14,6 +14,7 @@ public: void exit(); int checkChange(int cmd); private: + bool _setCorrectly; JointSpaceTraj *_toStartTraj; bool _reachedStart = false; bool _finishedRepeat = false; diff --git a/include/FSM/State_ToState.h b/include/FSM/State_ToState.h index 228d137..7b1a82e 100755 --- a/include/FSM/State_ToState.h +++ b/include/FSM/State_ToState.h @@ -13,6 +13,7 @@ public: void exit(); int checkChange(int cmd); private: + bool _setCorrectly; double _costTime; HomoMat _goalHomo; JointSpaceTraj *_jointTraj; diff --git a/include/FSM/State_Trajectory.h b/include/FSM/State_Trajectory.h index e0ad84b..8bad4c7 100755 --- a/include/FSM/State_Trajectory.h +++ b/include/FSM/State_Trajectory.h @@ -13,11 +13,12 @@ public: ~State_Trajectory(); void enter(); void run(); - virtual void exit(); - virtual int checkChange(int cmd); + void exit(); + int checkChange(int cmd); protected: // EndHomoTraj *_traj; virtual void _setTraj(); + virtual void _setTrajSdk(); TrajectoryManager *_traj; HomoMat _goalHomo; Vec6 _goalTwist; @@ -31,6 +32,10 @@ protected: std::vector _lineTraj; long long startTime; + static std::vector _trajCmd; + Vec6 _posture[2]; + + static bool _isTrajFSM; }; #endif // CARTESIANPATH_H \ No newline at end of file diff --git a/include/common/enumClass.h b/include/common/enumClass.h index 42dc334..9786c46 100755 --- a/include/common/enumClass.h +++ b/include/common/enumClass.h @@ -1,41 +1,6 @@ #ifndef ENUMCLASS_H #define ENUMCLASS_H -// enum class UserCommand{ -// // EXIT, -// NONE, -// START, -// L2_A, //used by robot -// L2_B, //used by robot -// L2_X, //used by robot -// R2_A, //Arm: joint space control -// R2_X, //Arm: Cartesian -// R2_Y, // -// R2_temp, -// ARM_PASSIVE, //Arm: passive -// ARM_CHECKJOINT_0, -// ARM_TRAJECTORY_0, -// ARM_SAVESTATE_0, -// ARM_TEACH_0, -// ARM_TEACHREPEAT_0, -// ARM_NEXT, -// ARM_BACKTOSTART, - -// HEAD_JOINTCTRL, -// HEAD_TOSTART, -// HEAD_SAVE, -// HEAD_TURN, -// HEAD_CALI, -// HEAD_NEXT, -// HEAD_DANCE01, -// HEAD_DANCE02, -// HEAD_DANCE03, -// HEAD_DANCE04, -// HEAD_DANCE05, -// HEAD_DANCE06, -// HEAD_DANCE07 -// }; - enum class FrameType{ BODY, HIP, @@ -62,6 +27,7 @@ enum class ArmFSMStateName{ TEACH, TEACHREPEAT, CALIBRATION, + SETTRAJ, DANCE00, DANCE01, DANCE02, @@ -94,4 +60,10 @@ enum class HeadType{ DOG }; +enum class Control{ + _KEYBOARD, + _SDK, + _JOYSTICK, +}; + #endif // ENUMCLASS_H \ No newline at end of file diff --git a/include/common/math/mathTools.h b/include/common/math/mathTools.h index 00daeb0..59d35a8 100755 --- a/include/common/math/mathTools.h +++ b/include/common/math/mathTools.h @@ -2,7 +2,7 @@ #define MATHTOOLS_H #include -// #include "common/mathTypes.h" +#include "common/math/mathTypes.h" // template // inline T1 max(const T1 a, const T2 b){ @@ -60,14 +60,7 @@ inline double angleError(double first, double second, TurnDirection direction = double firstMod = fmod(first, 2.0*M_PI); double secondMod = fmod(second, 2.0*M_PI); - if(direction == TurnDirection::NOMATTER){ - if(fabs(firstMod - secondMod) > fabs(secondMod - firstMod)){ - return secondMod - firstMod; - }else{ - return firstMod - secondMod; - } - } - else if(direction == TurnDirection::POSITIVE){ + if(direction == TurnDirection::POSITIVE){ if(firstMod - secondMod < 0.0){ return 2*M_PI + firstMod - secondMod; }else{ @@ -80,6 +73,12 @@ inline double angleError(double first, double second, TurnDirection direction = }else{ return firstMod - secondMod; } + }else{//no matter + if(fabs(firstMod - secondMod) > fabs(secondMod - firstMod)){ + return secondMod - firstMod; + }else{ + return firstMod - secondMod; + } } } @@ -478,5 +477,4 @@ private: double _zoomFactor; std::string _valueName; }; - -#endif // MATHTOOLS_H \ No newline at end of file +#endif \ No newline at end of file diff --git a/include/common/math/robotics.h b/include/common/math/robotics.h new file mode 100644 index 0000000..98cbd0d --- /dev/null +++ b/include/common/math/robotics.h @@ -0,0 +1,685 @@ +#pragma once + +#include +#include "common/math/mathTools.h" + +namespace mr { + +/* + * Function: Find if the value is negligible enough to consider 0 + * Inputs: value to be checked as a double + * Returns: Boolean of true-ignore or false-can't ignore + */ +bool NearZero(const double); + +/* + * Function: Calculate the 6x6 matrix [adV] of the given 6-vector + * Input: Eigen::VectorXd (6x1) + * Output: Eigen::MatrixXd (6x6) + * Note: Can be used to calculate the Lie bracket [V1, V2] = [adV1]V2 + */ +Eigen::MatrixXd ad(Eigen::VectorXd); + + +/* + * Function: Returns a normalized version of the input vector + * Input: Eigen::MatrixXd + * Output: Eigen::MatrixXd + * Note: MatrixXd is used instead of VectorXd for the case of row vectors + * Requires a copy + * Useful because of the MatrixXd casting + */ +Eigen::MatrixXd Normalize(Eigen::MatrixXd); + + +/* + * Function: Returns the skew symmetric matrix representation of an angular velocity vector + * Input: Eigen::Vector3d 3x1 angular velocity vector + * Returns: Eigen::MatrixXd 3x3 skew symmetric matrix + */ +Eigen::Matrix3d VecToso3(const Eigen::Vector3d&); + + +/* + * Function: Returns angular velocity vector represented by the skew symmetric matrix + * Inputs: Eigen::MatrixXd 3x3 skew symmetric matrix + * Returns: Eigen::Vector3d 3x1 angular velocity + */ +Eigen::Vector3d so3ToVec(const Eigen::MatrixXd&); + + +/* + * Function: Tranlates an exponential rotation into it's individual components + * Inputs: Exponential rotation (rotation matrix in terms of a rotation axis + * and the angle of rotation) + * Returns: The axis and angle of rotation as [x, y, z, theta] + */ +Eigen::Vector4d AxisAng3(const Eigen::Vector3d&); + + +/* + * Function: Translates an exponential rotation into a rotation matrix + * Inputs: exponenential representation of a rotation + * Returns: Rotation matrix + */ +Eigen::Matrix3d MatrixExp3(const Eigen::Matrix3d&); + + +/* Function: Computes the matrix logarithm of a rotation matrix + * Inputs: Rotation matrix + * Returns: matrix logarithm of a rotation + */ +Eigen::Matrix3d MatrixLog3(const Eigen::Matrix3d&); + + +/* + * Function: Combines a rotation matrix and position vector into a single + * Special Euclidian Group (SE3) homogeneous transformation matrix + * Inputs: Rotation Matrix (R), Position Vector (p) + * Returns: Matrix of T = [ [R, p], + * [0, 1] ] + */ +Eigen::MatrixXd RpToTrans(const Eigen::Matrix3d&, const Eigen::Vector3d&); + + +/* + * Function: Separates the rotation matrix and position vector from + * the transfomation matrix representation + * Inputs: Homogeneous transformation matrix + * Returns: std::vector of [rotation matrix, position vector] + */ +std::vector TransToRp(const Eigen::MatrixXd&); + + +/* + * Function: Translates a spatial velocity vector into a transformation matrix + * Inputs: Spatial velocity vector [angular velocity, linear velocity] + * Returns: Transformation matrix + */ +Eigen::MatrixXd VecTose3(const Eigen::VectorXd&); + + +/* Function: Translates a transformation matrix into a spatial velocity vector + * Inputs: Transformation matrix + * Returns: Spatial velocity vector [angular velocity, linear velocity] + */ +Eigen::VectorXd se3ToVec(const Eigen::MatrixXd&); + + +/* + * Function: Provides the adjoint representation of a transformation matrix + * Used to change the frame of reference for spatial velocity vectors + * Inputs: 4x4 Transformation matrix SE(3) + * Returns: 6x6 Adjoint Representation of the matrix + */ +Eigen::MatrixXd Adjoint(const Eigen::MatrixXd&); + + +/* + * Function: Rotation expanded for screw axis + * Inputs: se3 matrix representation of exponential coordinates (transformation matrix) + * Returns: 6x6 Matrix representing the rotation + */ +Eigen::MatrixXd MatrixExp6(const Eigen::MatrixXd&); + + +/* + * Function: Computes the matrix logarithm of a homogeneous transformation matrix + * Inputs: R: Transformation matrix in SE3 + * Returns: The matrix logarithm of R + */ +Eigen::MatrixXd MatrixLog6(const Eigen::MatrixXd&); + + +/* + * Function: Compute end effector frame (used for current spatial position calculation) + * Inputs: Home configuration (position and orientation) of end-effector + * The joint screw axes in the space frame when the manipulator + * is at the home position + * A list of joint coordinates. + * Returns: Transfomation matrix representing the end-effector frame when the joints are + * at the specified coordinates + * Notes: FK means Forward Kinematics + */ +Eigen::MatrixXd FKinSpace(const Eigen::MatrixXd&, const Eigen::MatrixXd&, const Eigen::VectorXd&); + +/* + * Function: Compute end effector frame (used for current body position calculation) + * Inputs: Home configuration (position and orientation) of end-effector + * The joint screw axes in the body frame when the manipulator + * is at the home position + * A list of joint coordinates. + * Returns: Transfomation matrix representing the end-effector frame when the joints are + * at the specified coordinates + * Notes: FK means Forward Kinematics + */ +Eigen::MatrixXd FKinBody(const Eigen::MatrixXd&, const Eigen::MatrixXd&, const Eigen::VectorXd&); + + +/* + * Function: Gives the space Jacobian + * Inputs: Screw axis in home position, joint configuration + * Returns: 6xn Spatial Jacobian + */ +Eigen::MatrixXd JacobianSpace(const Eigen::MatrixXd&, const Eigen::MatrixXd&); + + +/* + * Function: Gives the body Jacobian + * Inputs: Screw axis in BODY position, joint configuration + * Returns: 6xn Bobdy Jacobian + */ +Eigen::MatrixXd JacobianBody(const Eigen::MatrixXd&, const Eigen::MatrixXd&); + + +/* + * Inverts a homogeneous transformation matrix + * Inputs: A homogeneous transformation Matrix T + * Returns: The inverse of T + */ +Eigen::MatrixXd TransInv(const Eigen::MatrixXd&); + +/* + * Inverts a rotation matrix + * Inputs: A rotation matrix R + * Returns: The inverse of R + */ +Eigen::MatrixXd RotInv(const Eigen::MatrixXd&); + +/* + * Takes a parametric description of a screw axis and converts it to a + * normalized screw axis + * Inputs: + * q: A point lying on the screw axis + * s: A unit vector in the direction of the screw axis + * h: The pitch of the screw axis + * Returns: A normalized screw axis described by the inputs + */ +Eigen::VectorXd ScrewToAxis(Eigen::Vector3d q, Eigen::Vector3d s, double h); + + +/* + * Function: Translates a 6-vector of exponential coordinates into screw + * axis-angle form + * Inputs: + * expc6: A 6-vector of exponential coordinates for rigid-body motion + S*theta + * Returns: The corresponding normalized screw axis S; The distance theta traveled + * along/about S in form [S, theta] + * Note: Is it better to return std::map? + */ +Eigen::VectorXd AxisAng6(const Eigen::VectorXd&); + + +/* + * Function: Returns projection of one matrix into SO(3) + * Inputs: + * M: A matrix near SO(3) to project to SO(3) + * Returns: The closest matrix R that is in SO(3) + * Projects a matrix mat to the closest matrix in SO(3) using singular-value decomposition + * (see http://hades.mech.northwestern.edu/index.php/Modern_Robotics_Linear_Algebra_Review). + * This function is only appropriate for matrices close to SO(3). + */ +Eigen::MatrixXd ProjectToSO3(const Eigen::MatrixXd&); + + +/* + * Function: Returns projection of one matrix into SE(3) + * Inputs: + * M: A 4x4 matrix near SE(3) to project to SE(3) + * Returns: The closest matrix T that is in SE(3) + * Projects a matrix mat to the closest matrix in SO(3) using singular-value decomposition + * (see http://hades.mech.northwestern.edu/index.php/Modern_Robotics_Linear_Algebra_Review). + * This function is only appropriate for matrices close to SE(3). + */ +Eigen::MatrixXd ProjectToSE3(const Eigen::MatrixXd&); + + +/* + * Function: Returns the Frobenius norm to describe the distance of M from the SO(3) manifold + * Inputs: + * M: A 3x3 matrix + * Outputs: + * the distance from mat to the SO(3) manifold using the following + * method: + * If det(M) <= 0, return a large number. + * If det(M) > 0, return norm(M^T*M - I). + */ +double DistanceToSO3(const Eigen::Matrix3d&); + + +/* + * Function: Returns the Frobenius norm to describe the distance of mat from the SE(3) manifold + * Inputs: + * T: A 4x4 matrix + * Outputs: + * the distance from T to the SE(3) manifold using the following + * method: + * Compute the determinant of matR, the top 3x3 submatrix of T. + * If det(matR) <= 0, return a large number. + * If det(matR) > 0, replace the top 3x3 submatrix of mat with matR^T*matR, + * and set the first three entries of the fourth column of mat to zero. Then + * return norm(T - I). + */ +double DistanceToSE3(const Eigen::Matrix4d&); + + +/* + * Function: Returns true if M is close to or on the manifold SO(3) + * Inputs: + * M: A 3x3 matrix + * Outputs: + * true if M is very close to or in SO(3), false otherwise + */ +bool TestIfSO3(const Eigen::Matrix3d&); + + +/* + * Function: Returns true if T is close to or on the manifold SE(3) + * Inputs: + * M: A 4x4 matrix + * Outputs: + * true if T is very close to or in SE(3), false otherwise + */ +bool TestIfSE3(const Eigen::Matrix4d&); + + +/* + * Function: Computes inverse kinematics in the body frame for an open chain robot + * Inputs: + * Blist: The joint screw axes in the end-effector frame when the + * manipulator is at the home position, in the format of a + * matrix with axes as the columns + * M: The home configuration of the end-effector + * T: The desired end-effector configuration Tsd + * thetalist[in][out]: An initial guess and result output of joint angles that are close to + * satisfying Tsd + * emog: A small positive tolerance on the end-effector orientation + * error. The returned joint angles must give an end-effector + * orientation error less than eomg + * ev: A small positive tolerance on the end-effector linear position + * error. The returned joint angles must give an end-effector + * position error less than ev + * Outputs: + * success: A logical value where TRUE means that the function found + * a solution and FALSE means that it ran through the set + * number of maximum iterations without finding a solution + * within the tolerances eomg and ev. + * thetalist[in][out]: Joint angles that achieve T within the specified tolerances, + */ +bool IKinBody(const Eigen::MatrixXd&, const Eigen::MatrixXd&, const Eigen::MatrixXd&, Eigen::VectorXd&, double, double); + + +/* + * Function: Computes inverse kinematics in the space frame for an open chain robot + * Inputs: + * Slist: The joint screw axes in the space frame when the + * manipulator is at the home position, in the format of a + * matrix with axes as the columns + * M: The home configuration of the end-effector + * T: The desired end-effector configuration Tsd + * thetalist[in][out]: An initial guess and result output of joint angles that are close to + * satisfying Tsd + * emog: A small positive tolerance on the end-effector orientation + * error. The returned joint angles must give an end-effector + * orientation error less than eomg + * ev: A small positive tolerance on the end-effector linear position + * error. The returned joint angles must give an end-effector + * position error less than ev + * Outputs: + * success: A logical value where TRUE means that the function found + * a solution and FALSE means that it ran through the set + * number of maximum iterations without finding a solution + * within the tolerances eomg and ev. + * thetalist[in][out]: Joint angles that achieve T within the specified tolerances, + */ +bool IKinSpace(const Eigen::MatrixXd&, const Eigen::MatrixXd&, const Eigen::MatrixXd&, Eigen::VectorXd&, double, double); + +/* + * Function: This function uses forward-backward Newton-Euler iterations to solve the + * equation: + * taulist = Mlist(thetalist) * ddthetalist + c(thetalist, dthetalist) ... + * + g(thetalist) + Jtr(thetalist) * Ftip + * Inputs: + * thetalist: n-vector of joint variables + * dthetalist: n-vector of joint rates + * ddthetalist: n-vector of joint accelerations + * g: Gravity vector g + * Ftip: Spatial force applied by the end-effector expressed in frame {n+1} + * Mlist: List of link frames {i} relative to {i-1} at the home position + * Glist: Spatial inertia matrices Gi of the links + * Slist: Screw axes Si of the joints in a space frame, in the format + * of a matrix with the screw axes as the columns. + * + * Outputs: + * taulist: The n-vector of required joint forces/torques + * + */ +Eigen::VectorXd InverseDynamics(const Eigen::VectorXd&, const Eigen::VectorXd&, const Eigen::VectorXd&, + const Eigen::VectorXd&, const Eigen::VectorXd&, const std::vector&, + const std::vector&, const Eigen::MatrixXd&); + +/* + * Function: This function calls InverseDynamics with Ftip = 0, dthetalist = 0, and + * ddthetalist = 0. The purpose is to calculate one important term in the dynamics equation + * Inputs: + * thetalist: n-vector of joint variables + * g: Gravity vector g + * Mlist: List of link frames {i} relative to {i-1} at the home position + * Glist: Spatial inertia matrices Gi of the links + * Slist: Screw axes Si of the joints in a space frame, in the format + * of a matrix with the screw axes as the columns. + * + * Outputs: + * grav: The 3-vector showing the effect force of gravity to the dynamics + * + */ +Eigen::VectorXd GravityForces(const Eigen::VectorXd&, const Eigen::VectorXd&, + const std::vector&, const std::vector&, const Eigen::MatrixXd&); + +/* + * Function: This function calls InverseDynamics n times, each time passing a + * ddthetalist vector with a single element equal to one and all other + * inputs set to zero. Each call of InverseDynamics generates a single + * column, and these columns are assembled to create the inertia matrix. + * + * Inputs: + * thetalist: n-vector of joint variables + * Mlist: List of link frames {i} relative to {i-1} at the home position + * Glist: Spatial inertia matrices Gi of the links + * Slist: Screw axes Si of the joints in a space frame, in the format + * of a matrix with the screw axes as the columns. + * + * Outputs: + * M: The numerical inertia matrix M(thetalist) of an n-joint serial + * chain at the given configuration thetalist. + */ +Eigen::MatrixXd MassMatrix(const Eigen::VectorXd&, + const std::vector&, const std::vector&, const Eigen::MatrixXd&); + +/* + * Function: This function calls InverseDynamics with g = 0, Ftip = 0, and + * ddthetalist = 0. + * + * Inputs: + * thetalist: n-vector of joint variables + * dthetalist: A list of joint rates + * Mlist: List of link frames {i} relative to {i-1} at the home position + * Glist: Spatial inertia matrices Gi of the links + * Slist: Screw axes Si of the joints in a space frame, in the format + * of a matrix with the screw axes as the columns. + * + * Outputs: + * c: The vector c(thetalist,dthetalist) of Coriolis and centripetal + * terms for a given thetalist and dthetalist. + */ +Eigen::VectorXd VelQuadraticForces(const Eigen::VectorXd&, const Eigen::VectorXd&, + const std::vector&, const std::vector&, const Eigen::MatrixXd&); + +/* + * Function: This function calls InverseDynamics with g = 0, dthetalist = 0, and + * ddthetalist = 0. + * + * Inputs: + * thetalist: n-vector of joint variables + * Ftip: Spatial force applied by the end-effector expressed in frame {n+1} + * Mlist: List of link frames {i} relative to {i-1} at the home position + * Glist: Spatial inertia matrices Gi of the links + * Slist: Screw axes Si of the joints in a space frame, in the format + * of a matrix with the screw axes as the columns. + * + * Outputs: + * JTFtip: The joint forces and torques required only to create the + * end-effector force Ftip. + */ +Eigen::VectorXd EndEffectorForces(const Eigen::VectorXd&, const Eigen::VectorXd&, + const std::vector&, const std::vector&, const Eigen::MatrixXd&); + +/* + * Function: This function computes ddthetalist by solving: + * Mlist(thetalist) * ddthetalist = taulist - c(thetalist,dthetalist) + * - g(thetalist) - Jtr(thetalist) * Ftip + * Inputs: + * thetalist: n-vector of joint variables + * dthetalist: n-vector of joint rates + * taulist: An n-vector of joint forces/torques + * g: Gravity vector g + * Ftip: Spatial force applied by the end-effector expressed in frame {n+1} + * Mlist: List of link frames {i} relative to {i-1} at the home position + * Glist: Spatial inertia matrices Gi of the links + * Slist: Screw axes Si of the joints in a space frame, in the format + * of a matrix with the screw axes as the columns. + * + * Outputs: + * ddthetalist: The resulting joint accelerations + * + */ +Eigen::VectorXd ForwardDynamics(const Eigen::VectorXd&, const Eigen::VectorXd&, const Eigen::VectorXd&, + const Eigen::VectorXd&, const Eigen::VectorXd&, const std::vector&, + const std::vector&, const Eigen::MatrixXd&); + + +/* + * Function: Compute the joint angles and velocities at the next timestep using + first order Euler integration + * Inputs: + * thetalist[in]: n-vector of joint variables + * dthetalist[in]: n-vector of joint rates + * ddthetalist: n-vector of joint accelerations + * dt: The timestep delta t + * + * Outputs: + * thetalist[out]: Vector of joint variables after dt from first order Euler integration + * dthetalist[out]: Vector of joint rates after dt from first order Euler integration + */ +void EulerStep(Eigen::VectorXd&, Eigen::VectorXd&, const Eigen::VectorXd&, double); + + +/* + * Function: Compute the joint forces/torques required to move the serial chain along the given + * trajectory using inverse dynamics + * Inputs: + * thetamat: An N x n matrix of robot joint variables (N: no. of trajecoty time step points; n: no. of robot joints + * dthetamat: An N x n matrix of robot joint velocities + * ddthetamat: An N x n matrix of robot joint accelerations + * g: Gravity vector g + * Ftipmat: An N x 6 matrix of spatial forces applied by the end-effector (if there are no tip forces + * the user should input a zero matrix) + * Mlist: List of link frames {i} relative to {i-1} at the home position + * Glist: Spatial inertia matrices Gi of the links + * Slist: Screw axes Si of the joints in a space frame, in the format + * of a matrix with the screw axes as the columns. + * + * Outputs: + * taumat: The N x n matrix of joint forces/torques for the specified trajectory, where each of the N rows is the vector + * of joint forces/torques at each time step + */ +Eigen::MatrixXd InverseDynamicsTrajectory(const Eigen::MatrixXd&, const Eigen::MatrixXd&, const Eigen::MatrixXd&, + const Eigen::VectorXd&, const Eigen::MatrixXd&, const std::vector&, const std::vector&, + const Eigen::MatrixXd&); + + +/* + * Function: Compute the motion of a serial chain given an open-loop history of joint forces/torques + * Inputs: + * thetalist: n-vector of initial joint variables + * dthetalist: n-vector of initial joint rates + * taumat: An N x n matrix of joint forces/torques, where each row is is the joint effort at any time step + * g: Gravity vector g + * Ftipmat: An N x 6 matrix of spatial forces applied by the end-effector (if there are no tip forces + * the user should input a zero matrix) + * Mlist: List of link frames {i} relative to {i-1} at the home position + * Glist: Spatial inertia matrices Gi of the links + * Slist: Screw axes Si of the joints in a space frame, in the format + * of a matrix with the screw axes as the columns. + * dt: The timestep between consecutive joint forces/torques + * intRes: Integration resolution is the number of times integration (Euler) takes places between each time step. + * Must be an integer value greater than or equal to 1 + * + * Outputs: std::vector of [thetamat, dthetamat] + * thetamat: The N x n matrix of joint angles resulting from the specified joint forces/torques + * dthetamat: The N x n matrix of joint velocities + */ +std::vector ForwardDynamicsTrajectory(const Eigen::VectorXd&, const Eigen::VectorXd&, const Eigen::MatrixXd&, + const Eigen::VectorXd&, const Eigen::MatrixXd&, const std::vector&, const std::vector&, + const Eigen::MatrixXd&, double, int); + + +/* + * Function: Compute the joint control torques at a particular time instant + * Inputs: + * thetalist: n-vector of joint variables + * dthetalist: n-vector of joint rates + * eint: n-vector of the time-integral of joint errors + * g: Gravity vector g + * Mlist: List of link frames {i} relative to {i-1} at the home position + * Glist: Spatial inertia matrices Gi of the links + * Slist: Screw axes Si of the joints in a space frame, in the format + * of a matrix with the screw axes as the columns. + * thetalistd: n-vector of reference joint variables + * dthetalistd: n-vector of reference joint rates + * ddthetalistd: n-vector of reference joint accelerations + * Kp: The feedback proportional gain (identical for each joint) + * Ki: The feedback integral gain (identical for each joint) + * Kd: The feedback derivative gain (identical for each joint) + * + * Outputs: + * tau_computed: The vector of joint forces/torques computed by the feedback + * linearizing controller at the current instant + */ +Eigen::VectorXd ComputedTorque(const Eigen::VectorXd&, const Eigen::VectorXd&, const Eigen::VectorXd&, + const Eigen::VectorXd&, const std::vector&, const std::vector&, + const Eigen::MatrixXd&, const Eigen::VectorXd&, const Eigen::VectorXd&, const Eigen::VectorXd&, double, double, double); + + +/* + * Function: Compute s(t) for a cubic time scaling + * Inputs: + * Tf: Total time of the motion in seconds from rest to rest + * t: The current time t satisfying 0 < t < Tf + * + * Outputs: + * st: The path parameter corresponding to a third-order + * polynomial motion that begins and ends at zero velocity + */ +double CubicTimeScaling(double, double); + + +/* + * Function: Compute s(t) for a quintic time scaling + * Inputs: + * Tf: Total time of the motion in seconds from rest to rest + * t: The current time t satisfying 0 < t < Tf + * + * Outputs: + * st: The path parameter corresponding to a fifth-order + * polynomial motion that begins and ends at zero velocity + * and zero acceleration + */ +double QuinticTimeScaling(double, double); + + +/* + * Function: Compute a straight-line trajectory in joint space + * Inputs: + * thetastart: The initial joint variables + * thetaend: The final joint variables + * Tf: Total time of the motion in seconds from rest to rest + * N: The number of points N > 1 (Start and stop) in the discrete + * representation of the trajectory + * method: The time-scaling method, where 3 indicates cubic (third- + * order polynomial) time scaling and 5 indicates quintic + * (fifth-order polynomial) time scaling + * + * Outputs: + * traj: A trajectory as an N x n matrix, where each row is an n-vector + * of joint variables at an instant in time. The first row is + * thetastart and the Nth row is thetaend . The elapsed time + * between each row is Tf / (N - 1) + */ +Eigen::MatrixXd JointTrajectory(const Eigen::VectorXd&, const Eigen::VectorXd&, double, int, int); + + +/* + * Function: Compute a trajectory as a list of N SE(3) matrices corresponding to + * the screw motion about a space screw axis + * Inputs: + * Xstart: The initial end-effector configuration + * Xend: The final end-effector configuration + * Tf: Total time of the motion in seconds from rest to rest + * N: The number of points N > 1 (Start and stop) in the discrete + * representation of the trajectory + * method: The time-scaling method, where 3 indicates cubic (third- + * order polynomial) time scaling and 5 indicates quintic + * (fifth-order polynomial) time scaling + * + * Outputs: + * traj: The discretized trajectory as a list of N matrices in SE(3) + * separated in time by Tf/(N-1). The first in the list is Xstart + * and the Nth is Xend + */ +std::vector ScrewTrajectory(const Eigen::MatrixXd&, const Eigen::MatrixXd&, double, int, int); + + +/* + * Function: Compute a trajectory as a list of N SE(3) matrices corresponding to + * the origin of the end-effector frame following a straight line + * Inputs: + * Xstart: The initial end-effector configuration + * Xend: The final end-effector configuration + * Tf: Total time of the motion in seconds from rest to rest + * N: The number of points N > 1 (Start and stop) in the discrete + * representation of the trajectory + * method: The time-scaling method, where 3 indicates cubic (third- + * order polynomial) time scaling and 5 indicates quintic + * (fifth-order polynomial) time scaling + * + * Outputs: + * traj: The discretized trajectory as a list of N matrices in SE(3) + * separated in time by Tf/(N-1). The first in the list is Xstart + * and the Nth is Xend + * Notes: + * This function is similar to ScrewTrajectory, except the origin of the + * end-effector frame follows a straight line, decoupled from the rotational + * motion. + */ +std::vector CartesianTrajectory(const Eigen::MatrixXd&, const Eigen::MatrixXd&, double, int, int); + + +/* + * Function: Compute the motion of a serial chain given an open-loop history of joint forces/torques + * Inputs: + * thetalist: n-vector of initial joint variables + * dthetalist: n-vector of initial joint rates + * g: Gravity vector g + * Ftipmat: An N x 6 matrix of spatial forces applied by the end-effector (if there are no tip forces + * the user should input a zero matrix) + * Mlist: List of link frames {i} relative to {i-1} at the home position + * Glist: Spatial inertia matrices Gi of the links + * Slist: Screw axes Si of the joints in a space frame, in the format + * of a matrix with the screw axes as the columns. + * thetamatd: An Nxn matrix of desired joint variables from the reference trajectory + * dthetamatd: An Nxn matrix of desired joint velocities + * ddthetamatd: An Nxn matrix of desired joint accelerations + * gtilde: The gravity vector based on the model of the actual robot (actual values given above) + * Mtildelist: The link frame locations based on the model of the actual robot (actual values given above) + * Gtildelist: The link spatial inertias based on the model of the actual robot (actual values given above) + * Kp: The feedback proportional gain (identical for each joint) + * Ki: The feedback integral gain (identical for each joint) + * Kd: The feedback derivative gain (identical for each joint) + * dt: The timestep between points on the reference trajectory + * intRes: Integration resolution is the number of times integration (Euler) takes places between each time step. + * Must be an integer value greater than or equal to 1 + * + * Outputs: std::vector of [taumat, thetamat] + * taumat: An Nxn matrix of the controllers commanded joint forces/ torques, where each row of n forces/torques + * corresponds to a single time instant + * thetamat: The N x n matrix of actual joint angles + */ +std::vector SimulateControl(const Eigen::VectorXd&, const Eigen::VectorXd&, const Eigen::VectorXd&, + const Eigen::MatrixXd&, const std::vector&, const std::vector&, + const Eigen::MatrixXd&, const Eigen::MatrixXd&, const Eigen::MatrixXd&, const Eigen::MatrixXd&, + const Eigen::VectorXd&, const std::vector&, const std::vector&, + double, double, double, double, int); + +} diff --git a/include/common/utilities/CSVTool.h b/include/common/utilities/CSVTool.h index c8497ec..bc27544 100755 --- a/include/common/utilities/CSVTool.h +++ b/include/common/utilities/CSVTool.h @@ -6,7 +6,6 @@ only suitable for small .csv file. for large .csv, try (read only) */ - #include #include #include @@ -61,6 +60,7 @@ public: void readFile(); void saveFile(); + bool _hasFile; private: std::string _fileName; std::fstream _ioStream; @@ -139,10 +139,13 @@ inline CSVTool::CSVTool(std::string fileName, FileType type, int precision) if(!_ioStream.is_open()){ std::cout << "[ERROR] CSVTool open file: " << fileName << " failed!" << std::endl; - exit(-1); + // exit(-1); + _hasFile = false; + }else{ + readFile(); + _hasFile = true; } - readFile(); } else if(type == FileType::CLEAR_DUMP){ _ioStream.open(_fileName, std::fstream::out); @@ -154,6 +157,7 @@ inline void CSVTool::readFile(){ if(!_ioStream.is_open()){ // _ioStream.open(_fileName, std::fstream::ate | std::fstream::in | std::fstream::out); std::cout << "[ERROR] CSVTool::readFile, file: " << _fileName << " has not been opened!" << std::endl; + return; } _lines.clear(); @@ -177,7 +181,7 @@ inline void CSVTool::readFile(){ inline bool CSVTool::getLine(std::string label, std::vector &values){ if(_labels.count(label) == 0){ - std::cout << "[ERROR] No such label: " << label << std::endl; + // std::cout << "[ERROR] No such label: " << label << std::endl; return false; }else{ _lines.at(_labels[label])->getValues(values); diff --git a/include/common/utilities/PyPlot.h b/include/common/utilities/PyPlot.h deleted file mode 100644 index fae55a5..0000000 --- a/include/common/utilities/PyPlot.h +++ /dev/null @@ -1,261 +0,0 @@ -#ifndef PYPLOT_H -#define PYPLOT_H - -#include -#include -#include -#include -#include -#include -#include "common/utilities/matplotlibcpp.h" -#include "unitree_arm_sdk/timer.h" - -namespace plt = matplotlibcpp; - -// enum class PlotType{ -// STATIC, -// ANIME -// }; - -struct Curve{ - std::vector x; - std::vector y; - - void printXY(double xRough, int pointNum){ - for(int i(0); i curves; - std::vector labels; - std::string plotName; - int curveCount; - - std::map curveName2ID; - - Plot(std::string name, int count, std::vector labelVec) - :plotName(name), curveCount(count), labels(labelVec){ - for(int i(0); i < count; ++i){ - curveName2ID.insert(std::pair(labels[i], i)); - curves.push_back(new Curve()); - } - } - - ~Plot(){ - for(int i(0); i < curveCount; ++i){ - delete curves[i]; - } - } - - double getX(long long startT){ - return (double)(getSystemTime() - startT) * 1e-6; - } - - void printXY(std::string curveName, double xRough, int pointNum){ - std::cout << "[DEBUG] Plot: " << plotName << ", Curve: " << curveName << std::endl; - curves[curveName2ID[curveName]]->printXY(xRough, pointNum); - } -}; - -class PyPlot{ -public: - PyPlot(); - ~PyPlot(); - void addPlot(std::string plotName, int curveCount, std::vector labelVec); - void addPlot(std::string plotName, int curveCount); - void showPlot(std::string plotName); - void showPlot(std::vector plotNameVec); - void showPlotAll(); - - void printXY(std::string plotName, std::string curveName, double xRough, int pointNum = 1); - - void addFrame(std::string plotName, double value); - void addFrame(std::string plotName, double x, double value); - - template - void addFrame(std::string plotName, T* valueArray); - template - void addFrame(std::string plotName, double x, T* valueArray); - - template - void addFrame(std::string plotName, const Eigen::MatrixBase &vec); - template - void addFrame(std::string plotName, double x, const Eigen::MatrixBase &vec); - - template - void addFrame(std::string plotName, const std::vector &vec); - template - void addFrame(std::string plotName, double x, const std::vector &vec); - -private: - void _checkStart(); - int _plotCount = 0; - std::map _plotName2ID; - std::vector< Plot* > _plots; - // std::thread _plotThread; - long long _pointNum; - Plot* _getPlotPtr(std::string plotName); - bool start; - long long startT; -}; - -inline PyPlot::PyPlot(){ - start = false; - // _plotThread = std::thread(&PyPlot::_plotThreadFunc, this); -} - -inline PyPlot::~PyPlot(){ - for(int i(0); i < _plotCount; ++i){ - delete _plots[i]; - } -} - -inline void PyPlot::_checkStart(){ - if(!start){ - start = true; - startT = getSystemTime(); - } -} - -inline void PyPlot::printXY(std::string plotName, std::string curveName, double xRough, int pointNum){ - _plots[_plotName2ID[plotName]]->printXY(curveName, xRough, pointNum); -} - -inline void PyPlot::addPlot(std::string plotName, int curveCount, std::vector labelVec){ - if(_plotName2ID.count(plotName) == 0){ - _plotName2ID.insert(std::pair(plotName, _plotCount)); - ++_plotCount; - - _plots.push_back( new Plot(plotName, curveCount, labelVec) ); - }else{ - std::cout << "[ERROR] Already has same Plot: " << plotName << std::endl; - exit(-1); - } -} - -inline void PyPlot::addPlot(std::string plotName, int curveCount){ - std::vector label; - for(int i(0); i < curveCount; ++i){ - label.push_back(std::to_string(i+1)); - } - addPlot(plotName, curveCount, label); -} - -inline void PyPlot::showPlot(std::string plotName){ - Plot* plot = _getPlotPtr(plotName); - plt::figure(); - plt::title(plot->plotName); - for(int i(0); i < plot->curveCount; ++i){ - plt::named_plot(plot->labels[i], plot->curves[i]->x, plot->curves[i]->y); - } - plt::legend(); - plt::show(); -} - -inline void PyPlot::showPlot(std::vector plotNameVec){ - for(std::vector::iterator itName = plotNameVec.begin(); itName != plotNameVec.end(); ++itName){ - plt::figure(); - Plot* plot = _plots[_plotName2ID[*itName]]; - plt::title(plot->plotName); - for(int i(0); i < plot->curveCount; ++i){ - plt::named_plot(plot->labels[i], plot->curves[i]->x, plot->curves[i]->y); - } - plt::legend(); - } - plt::show(); -} - -inline void PyPlot::showPlotAll(){ - for(int i(0); i < _plotCount; ++i){ - plt::figure(); - Plot* plot = _plots[i]; - plt::title(plot->plotName); - for(int j(0); j < plot->curveCount; ++j){ - plt::named_plot(plot->labels[j], plot->curves[j]->x, plot->curves[j]->y); - } - plt::legend(); - } - plt::show(); -} - -inline Plot* PyPlot::_getPlotPtr(std::string plotName){ - if(_plotName2ID.count(plotName) == 0){ - std::cout << "[ERROR] Plot " << plotName << " does not exist" << std::endl; - exit(-1); - }else{ - return _plots[_plotName2ID[plotName]]; - } -} - -inline void PyPlot::addFrame(std::string plotName, double value){ - _checkStart(); - Plot* plot = _getPlotPtr(plotName); - addFrame(plotName, plot->getX(startT), value); -} - -inline void PyPlot::addFrame(std::string plotName, double x, double value){ - Plot* plot = _getPlotPtr(plotName); - - plot->curves[0]->x.push_back(x); - plot->curves[0]->y.push_back(value); -} - -template -inline void PyPlot::addFrame(std::string plotName, T* valueArray){ - _checkStart(); - Plot* plot = _getPlotPtr(plotName); - addFrame(plotName, plot->getX(startT), valueArray); -} - -template -inline void PyPlot::addFrame(std::string plotName, double x, T* valueArray){ - Plot* plot = _getPlotPtr(plotName); - - for(int i(0); i < plot->curveCount; ++i){ - plot->curves[i]->x.push_back(x); - plot->curves[i]->y.push_back(valueArray[i]); - } -} - -template -inline void PyPlot::addFrame(std::string plotName, const Eigen::MatrixBase &vec){ - _checkStart(); - Plot* plot = _getPlotPtr(plotName); - addFrame(plotName, plot->getX(startT), vec); -} - -template -inline void PyPlot::addFrame(std::string plotName, double x, const Eigen::MatrixBase &vec){ - Plot* plot = _getPlotPtr(plotName); - - for(int i(0); i < plot->curveCount; ++i){ - plot->curves[i]->x.push_back(x); - plot->curves[i]->y.push_back(vec(i)); - } -} - -template -inline void PyPlot::addFrame(std::string plotName, const std::vector &vec){ - _checkStart(); - Plot* plot = _getPlotPtr(plotName); - addFrame(plotName, plot->getX(startT), vec); -} - -template -inline void PyPlot::addFrame(std::string plotName, double x, const std::vector &vec){ - Plot* plot = _getPlotPtr(plotName); - - for(int i(0); i < plot->curveCount; ++i){ - plot->curves[i]->x.push_back(x); - plot->curves[i]->y.push_back(vec[i]); - } -} -#endif // PYPLOT_H \ No newline at end of file diff --git a/include/common/utilities/matplotlibcpp.h b/include/common/utilities/matplotlibcpp.h deleted file mode 100755 index e017e47..0000000 --- a/include/common/utilities/matplotlibcpp.h +++ /dev/null @@ -1,2557 +0,0 @@ -#pragma once - -// Python headers must be included before any system headers, since -// they define _POSIX_C_SOURCE -#include - -#include -#include -#include -#include -#include -#include -#include -#include // requires c++11 support -#include - -#ifndef WITHOUT_NUMPY -# define NPY_NO_DEPRECATED_API NPY_1_7_API_VERSION -# include - -# ifdef WITH_OPENCV -# include -# endif // WITH_OPENCV - -/* - * A bunch of constants were removed in OpenCV 4 in favour of enum classes, so - * define the ones we need here. - */ -# if CV_MAJOR_VERSION > 3 -# define CV_BGR2RGB cv::COLOR_BGR2RGB -# define CV_BGRA2RGBA cv::COLOR_BGRA2RGBA -# endif -#endif // WITHOUT_NUMPY - -#if PY_MAJOR_VERSION >= 3 -# define PyString_FromString PyUnicode_FromString -# define PyInt_FromLong PyLong_FromLong -# define PyString_FromString PyUnicode_FromString -#endif - - -namespace matplotlibcpp { -namespace detail { - -static std::string s_backend; - -struct _interpreter { - PyObject* s_python_function_arrow; - PyObject *s_python_function_show; - PyObject *s_python_function_close; - PyObject *s_python_function_draw; - PyObject *s_python_function_pause; - PyObject *s_python_function_save; - PyObject *s_python_function_figure; - PyObject *s_python_function_fignum_exists; - PyObject *s_python_function_plot; - PyObject *s_python_function_quiver; - PyObject* s_python_function_contour; - PyObject *s_python_function_semilogx; - PyObject *s_python_function_semilogy; - PyObject *s_python_function_loglog; - PyObject *s_python_function_fill; - PyObject *s_python_function_fill_between; - PyObject *s_python_function_hist; - PyObject *s_python_function_imshow; - PyObject *s_python_function_scatter; - PyObject *s_python_function_boxplot; - PyObject *s_python_function_subplot; - PyObject *s_python_function_subplot2grid; - PyObject *s_python_function_legend; - PyObject *s_python_function_xlim; - PyObject *s_python_function_ion; - PyObject *s_python_function_ginput; - PyObject *s_python_function_ylim; - PyObject *s_python_function_title; - PyObject *s_python_function_axis; - PyObject *s_python_function_axvline; - PyObject *s_python_function_axvspan; - PyObject *s_python_function_xlabel; - PyObject *s_python_function_ylabel; - PyObject *s_python_function_gca; - PyObject *s_python_function_xticks; - PyObject *s_python_function_yticks; - PyObject* s_python_function_margins; - PyObject *s_python_function_tick_params; - PyObject *s_python_function_grid; - PyObject* s_python_function_cla; - PyObject *s_python_function_clf; - PyObject *s_python_function_errorbar; - PyObject *s_python_function_annotate; - PyObject *s_python_function_tight_layout; - PyObject *s_python_colormap; - PyObject *s_python_empty_tuple; - PyObject *s_python_function_stem; - PyObject *s_python_function_xkcd; - PyObject *s_python_function_text; - PyObject *s_python_function_suptitle; - PyObject *s_python_function_bar; - PyObject *s_python_function_barh; - PyObject *s_python_function_colorbar; - PyObject *s_python_function_subplots_adjust; - - - /* For now, _interpreter is implemented as a singleton since its currently not possible to have - multiple independent embedded python interpreters without patching the python source code - or starting a separate process for each. [1] - Furthermore, many python objects expect that they are destructed in the same thread as they - were constructed. [2] So for advanced usage, a `kill()` function is provided so that library - users can manually ensure that the interpreter is constructed and destroyed within the - same thread. - - 1: http://bytes.com/topic/python/answers/793370-multiple-independent-python-interpreters-c-c-program - 2: https://github.com/lava/matplotlib-cpp/pull/202#issue-436220256 - */ - - static _interpreter& get() { - return interkeeper(false); - } - - static _interpreter& kill() { - return interkeeper(true); - } - - // Stores the actual singleton object referenced by `get()` and `kill()`. - static _interpreter& interkeeper(bool should_kill) { - static _interpreter ctx; - if (should_kill) - ctx.~_interpreter(); - return ctx; - } - - PyObject* safe_import(PyObject* module, std::string fname) { - PyObject* fn = PyObject_GetAttrString(module, fname.c_str()); - - if (!fn) - throw std::runtime_error(std::string("Couldn't find required function: ") + fname); - - if (!PyFunction_Check(fn)) - throw std::runtime_error(fname + std::string(" is unexpectedly not a PyFunction.")); - - return fn; - } - -private: - -#ifndef WITHOUT_NUMPY -# if PY_MAJOR_VERSION >= 3 - - void *import_numpy() { - import_array(); // initialize C-API - return NULL; - } - -# else - - void import_numpy() { - import_array(); // initialize C-API - } - -# endif -#endif - - _interpreter() { - - // optional but recommended -#if PY_MAJOR_VERSION >= 3 - wchar_t name[] = L"plotting"; -#else - char name[] = "plotting"; -#endif - Py_SetProgramName(name); - Py_Initialize(); - - wchar_t const *dummy_args[] = {L"Python", NULL}; // const is needed because literals must not be modified - wchar_t const **argv = dummy_args; - int argc = sizeof(dummy_args)/sizeof(dummy_args[0])-1; - // Python3: - // PySys_SetArgv(argc, const_cast(argv)); - // Python2: - char** argm = (char **)(argv); PySys_SetArgv(argc, argm); - -#ifndef WITHOUT_NUMPY - import_numpy(); // initialize numpy C-API -#endif - - PyObject* matplotlibname = PyString_FromString("matplotlib"); - PyObject* pyplotname = PyString_FromString("matplotlib.pyplot"); - PyObject* cmname = PyString_FromString("matplotlib.cm"); - PyObject* pylabname = PyString_FromString("pylab"); - if (!pyplotname || !pylabname || !matplotlibname || !cmname) { - throw std::runtime_error("couldnt create string"); - } - - PyObject* matplotlib = PyImport_Import(matplotlibname); - Py_DECREF(matplotlibname); - if (!matplotlib) { - PyErr_Print(); - throw std::runtime_error("Error loading module matplotlib!"); - } - - // matplotlib.use() must be called *before* pylab, matplotlib.pyplot, - // or matplotlib.backends is imported for the first time - if (!s_backend.empty()) { - PyObject_CallMethod(matplotlib, const_cast("use"), const_cast("s"), s_backend.c_str()); - } - - PyObject* pymod = PyImport_Import(pyplotname); - Py_DECREF(pyplotname); - if (!pymod) { throw std::runtime_error("Error loading module matplotlib.pyplot!"); } - - s_python_colormap = PyImport_Import(cmname); - Py_DECREF(cmname); - if (!s_python_colormap) { throw std::runtime_error("Error loading module matplotlib.cm!"); } - - PyObject* pylabmod = PyImport_Import(pylabname); - Py_DECREF(pylabname); - if (!pylabmod) { throw std::runtime_error("Error loading module pylab!"); } - - s_python_function_arrow = safe_import(pymod, "arrow"); - s_python_function_show = safe_import(pymod, "show"); - s_python_function_close = safe_import(pymod, "close"); - s_python_function_draw = safe_import(pymod, "draw"); - s_python_function_pause = safe_import(pymod, "pause"); - s_python_function_figure = safe_import(pymod, "figure"); - s_python_function_fignum_exists = safe_import(pymod, "fignum_exists"); - s_python_function_plot = safe_import(pymod, "plot"); - s_python_function_quiver = safe_import(pymod, "quiver"); - s_python_function_contour = safe_import(pymod, "contour"); - s_python_function_semilogx = safe_import(pymod, "semilogx"); - s_python_function_semilogy = safe_import(pymod, "semilogy"); - s_python_function_loglog = safe_import(pymod, "loglog"); - s_python_function_fill = safe_import(pymod, "fill"); - s_python_function_fill_between = safe_import(pymod, "fill_between"); - s_python_function_hist = safe_import(pymod,"hist"); - s_python_function_scatter = safe_import(pymod,"scatter"); - s_python_function_boxplot = safe_import(pymod,"boxplot"); - s_python_function_subplot = safe_import(pymod, "subplot"); - s_python_function_subplot2grid = safe_import(pymod, "subplot2grid"); - s_python_function_legend = safe_import(pymod, "legend"); - s_python_function_ylim = safe_import(pymod, "ylim"); - s_python_function_title = safe_import(pymod, "title"); - s_python_function_axis = safe_import(pymod, "axis"); - s_python_function_axvline = safe_import(pymod, "axvline"); - s_python_function_axvspan = safe_import(pymod, "axvspan"); - s_python_function_xlabel = safe_import(pymod, "xlabel"); - s_python_function_ylabel = safe_import(pymod, "ylabel"); - s_python_function_gca = safe_import(pymod, "gca"); - s_python_function_xticks = safe_import(pymod, "xticks"); - s_python_function_yticks = safe_import(pymod, "yticks"); - s_python_function_margins = safe_import(pymod, "margins"); - s_python_function_tick_params = safe_import(pymod, "tick_params"); - s_python_function_grid = safe_import(pymod, "grid"); - s_python_function_xlim = safe_import(pymod, "xlim"); - s_python_function_ion = safe_import(pymod, "ion"); - s_python_function_ginput = safe_import(pymod, "ginput"); - s_python_function_save = safe_import(pylabmod, "savefig"); - s_python_function_annotate = safe_import(pymod,"annotate"); - s_python_function_cla = safe_import(pymod, "cla"); - s_python_function_clf = safe_import(pymod, "clf"); - s_python_function_errorbar = safe_import(pymod, "errorbar"); - s_python_function_tight_layout = safe_import(pymod, "tight_layout"); - s_python_function_stem = safe_import(pymod, "stem"); - s_python_function_xkcd = safe_import(pymod, "xkcd"); - s_python_function_text = safe_import(pymod, "text"); - s_python_function_suptitle = safe_import(pymod, "suptitle"); - s_python_function_bar = safe_import(pymod,"bar"); - s_python_function_barh = safe_import(pymod, "barh"); - s_python_function_colorbar = PyObject_GetAttrString(pymod, "colorbar"); - s_python_function_subplots_adjust = safe_import(pymod,"subplots_adjust"); -#ifndef WITHOUT_NUMPY - s_python_function_imshow = safe_import(pymod, "imshow"); -#endif - s_python_empty_tuple = PyTuple_New(0); - } - - ~_interpreter() { - Py_Finalize(); - } -}; - -} // end namespace detail - -/// Select the backend -/// -/// **NOTE:** This must be called before the first plot command to have -/// any effect. -/// -/// Mainly useful to select the non-interactive 'Agg' backend when running -/// matplotlibcpp in headless mode, for example on a machine with no display. -/// -/// See also: https://matplotlib.org/2.0.2/api/matplotlib_configuration_api.html#matplotlib.use -inline void backend(const std::string& name) -{ - detail::s_backend = name; -} - -inline bool annotate(std::string annotation, double x, double y) -{ - detail::_interpreter::get(); - - PyObject * xy = PyTuple_New(2); - PyObject * str = PyString_FromString(annotation.c_str()); - - PyTuple_SetItem(xy,0,PyFloat_FromDouble(x)); - PyTuple_SetItem(xy,1,PyFloat_FromDouble(y)); - - PyObject* kwargs = PyDict_New(); - PyDict_SetItemString(kwargs, "xy", xy); - - PyObject* args = PyTuple_New(1); - PyTuple_SetItem(args, 0, str); - - PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_annotate, args, kwargs); - - Py_DECREF(args); - Py_DECREF(kwargs); - - if(res) Py_DECREF(res); - - return res; -} - -namespace detail { - -#ifndef WITHOUT_NUMPY -// Type selector for numpy array conversion -template struct select_npy_type { const static NPY_TYPES type = NPY_NOTYPE; }; //Default -template <> struct select_npy_type { const static NPY_TYPES type = NPY_DOUBLE; }; -template <> struct select_npy_type { const static NPY_TYPES type = NPY_FLOAT; }; -template <> struct select_npy_type { const static NPY_TYPES type = NPY_BOOL; }; -template <> struct select_npy_type { const static NPY_TYPES type = NPY_INT8; }; -template <> struct select_npy_type { const static NPY_TYPES type = NPY_SHORT; }; -template <> struct select_npy_type { const static NPY_TYPES type = NPY_INT; }; -template <> struct select_npy_type { const static NPY_TYPES type = NPY_INT64; }; -template <> struct select_npy_type { const static NPY_TYPES type = NPY_UINT8; }; -template <> struct select_npy_type { const static NPY_TYPES type = NPY_USHORT; }; -template <> struct select_npy_type { const static NPY_TYPES type = NPY_ULONG; }; -template <> struct select_npy_type { const static NPY_TYPES type = NPY_UINT64; }; - -// Sanity checks; comment them out or change the numpy type below if you're compiling on -// a platform where they don't apply -static_assert(sizeof(long long) == 8); -template <> struct select_npy_type { const static NPY_TYPES type = NPY_INT64; }; -static_assert(sizeof(unsigned long long) == 8); -template <> struct select_npy_type { const static NPY_TYPES type = NPY_UINT64; }; -// TODO: add int, long, etc. - -template -PyObject* get_array(const std::vector& v) -{ - npy_intp vsize = v.size(); - NPY_TYPES type = select_npy_type::type; - if (type == NPY_NOTYPE) { - size_t memsize = v.size()*sizeof(double); - double* dp = static_cast(::malloc(memsize)); - for (size_t i=0; i(varray), NPY_ARRAY_OWNDATA); - return varray; - } - - PyObject* varray = PyArray_SimpleNewFromData(1, &vsize, type, (void*)(v.data())); - return varray; -} - - -template -PyObject* get_2darray(const std::vector<::std::vector>& v) -{ - if (v.size() < 1) throw std::runtime_error("get_2d_array v too small"); - - npy_intp vsize[2] = {static_cast(v.size()), - static_cast(v[0].size())}; - - PyArrayObject *varray = - (PyArrayObject *)PyArray_SimpleNew(2, vsize, NPY_DOUBLE); - - double *vd_begin = static_cast(PyArray_DATA(varray)); - - for (const ::std::vector &v_row : v) { - if (v_row.size() != static_cast(vsize[1])) - throw std::runtime_error("Missmatched array size"); - std::copy(v_row.begin(), v_row.end(), vd_begin); - vd_begin += vsize[1]; - } - - return reinterpret_cast(varray); -} - -#else // fallback if we don't have numpy: copy every element of the given vector - -template -PyObject* get_array(const std::vector& v) -{ - PyObject* list = PyList_New(v.size()); - for(size_t i = 0; i < v.size(); ++i) { - PyList_SetItem(list, i, PyFloat_FromDouble(v.at(i))); - } - return list; -} - -#endif // WITHOUT_NUMPY - -// sometimes, for labels and such, we need string arrays -inline PyObject * get_array(const std::vector& strings) -{ - PyObject* list = PyList_New(strings.size()); - for (std::size_t i = 0; i < strings.size(); ++i) { - PyList_SetItem(list, i, PyString_FromString(strings[i].c_str())); - } - return list; -} - -// not all matplotlib need 2d arrays, some prefer lists of lists -template -PyObject* get_listlist(const std::vector>& ll) -{ - PyObject* listlist = PyList_New(ll.size()); - for (std::size_t i = 0; i < ll.size(); ++i) { - PyList_SetItem(listlist, i, get_array(ll[i])); - } - return listlist; -} - -} // namespace detail - -/// Plot a line through the given x and y data points.. -/// -/// See: https://matplotlib.org/3.2.1/api/_as_gen/matplotlib.pyplot.plot.html -template -bool plot(const std::vector &x, const std::vector &y, const std::map& keywords) -{ - assert(x.size() == y.size()); - - detail::_interpreter::get(); - - // using numpy arrays - PyObject* xarray = detail::get_array(x); - PyObject* yarray = detail::get_array(y); - - // construct positional args - PyObject* args = PyTuple_New(2); - PyTuple_SetItem(args, 0, xarray); - PyTuple_SetItem(args, 1, yarray); - - // construct keyword args - PyObject* kwargs = PyDict_New(); - for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) - { - PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str())); - } - - PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_plot, args, kwargs); - - Py_DECREF(args); - Py_DECREF(kwargs); - if(res) Py_DECREF(res); - - return res; -} - -// TODO - it should be possible to make this work by implementing -// a non-numpy alternative for `detail::get_2darray()`. -#ifndef WITHOUT_NUMPY -template -void plot_surface(const std::vector<::std::vector> &x, - const std::vector<::std::vector> &y, - const std::vector<::std::vector> &z, - const std::map &keywords = - std::map()) -{ - detail::_interpreter::get(); - - // We lazily load the modules here the first time this function is called - // because I'm not sure that we can assume "matplotlib installed" implies - // "mpl_toolkits installed" on all platforms, and we don't want to require - // it for people who don't need 3d plots. - static PyObject *mpl_toolkitsmod = nullptr, *axis3dmod = nullptr; - if (!mpl_toolkitsmod) { - detail::_interpreter::get(); - - PyObject* mpl_toolkits = PyString_FromString("mpl_toolkits"); - PyObject* axis3d = PyString_FromString("mpl_toolkits.mplot3d"); - if (!mpl_toolkits || !axis3d) { throw std::runtime_error("couldnt create string"); } - - mpl_toolkitsmod = PyImport_Import(mpl_toolkits); - Py_DECREF(mpl_toolkits); - if (!mpl_toolkitsmod) { throw std::runtime_error("Error loading module mpl_toolkits!"); } - - axis3dmod = PyImport_Import(axis3d); - Py_DECREF(axis3d); - if (!axis3dmod) { throw std::runtime_error("Error loading module mpl_toolkits.mplot3d!"); } - } - - assert(x.size() == y.size()); - assert(y.size() == z.size()); - - // using numpy arrays - PyObject *xarray = detail::get_2darray(x); - PyObject *yarray = detail::get_2darray(y); - PyObject *zarray = detail::get_2darray(z); - - // construct positional args - PyObject *args = PyTuple_New(3); - PyTuple_SetItem(args, 0, xarray); - PyTuple_SetItem(args, 1, yarray); - PyTuple_SetItem(args, 2, zarray); - - // Build up the kw args. - PyObject *kwargs = PyDict_New(); - PyDict_SetItemString(kwargs, "rstride", PyInt_FromLong(1)); - PyDict_SetItemString(kwargs, "cstride", PyInt_FromLong(1)); - - PyObject *python_colormap_coolwarm = PyObject_GetAttrString( - detail::_interpreter::get().s_python_colormap, "coolwarm"); - - PyDict_SetItemString(kwargs, "cmap", python_colormap_coolwarm); - - for (std::map::const_iterator it = keywords.begin(); - it != keywords.end(); ++it) { - PyDict_SetItemString(kwargs, it->first.c_str(), - PyString_FromString(it->second.c_str())); - } - - - PyObject *fig = - PyObject_CallObject(detail::_interpreter::get().s_python_function_figure, - detail::_interpreter::get().s_python_empty_tuple); - if (!fig) throw std::runtime_error("Call to figure() failed."); - - PyObject *gca_kwargs = PyDict_New(); - PyDict_SetItemString(gca_kwargs, "projection", PyString_FromString("3d")); - - PyObject *gca = PyObject_GetAttrString(fig, "gca"); - if (!gca) throw std::runtime_error("No gca"); - Py_INCREF(gca); - PyObject *axis = PyObject_Call( - gca, detail::_interpreter::get().s_python_empty_tuple, gca_kwargs); - - if (!axis) throw std::runtime_error("No axis"); - Py_INCREF(axis); - - Py_DECREF(gca); - Py_DECREF(gca_kwargs); - - PyObject *plot_surface = PyObject_GetAttrString(axis, "plot_surface"); - if (!plot_surface) throw std::runtime_error("No surface"); - Py_INCREF(plot_surface); - PyObject *res = PyObject_Call(plot_surface, args, kwargs); - if (!res) throw std::runtime_error("failed surface"); - Py_DECREF(plot_surface); - - Py_DECREF(axis); - Py_DECREF(args); - Py_DECREF(kwargs); - if (res) Py_DECREF(res); -} -#endif // WITHOUT_NUMPY - -template -void plot3(const std::vector &x, - const std::vector &y, - const std::vector &z, - const std::map &keywords = - std::map()) -{ - detail::_interpreter::get(); - - // Same as with plot_surface: We lazily load the modules here the first time - // this function is called because I'm not sure that we can assume "matplotlib - // installed" implies "mpl_toolkits installed" on all platforms, and we don't - // want to require it for people who don't need 3d plots. - static PyObject *mpl_toolkitsmod = nullptr, *axis3dmod = nullptr; - if (!mpl_toolkitsmod) { - detail::_interpreter::get(); - - PyObject* mpl_toolkits = PyString_FromString("mpl_toolkits"); - PyObject* axis3d = PyString_FromString("mpl_toolkits.mplot3d"); - if (!mpl_toolkits || !axis3d) { throw std::runtime_error("couldnt create string"); } - - mpl_toolkitsmod = PyImport_Import(mpl_toolkits); - Py_DECREF(mpl_toolkits); - if (!mpl_toolkitsmod) { throw std::runtime_error("Error loading module mpl_toolkits!"); } - - axis3dmod = PyImport_Import(axis3d); - Py_DECREF(axis3d); - if (!axis3dmod) { throw std::runtime_error("Error loading module mpl_toolkits.mplot3d!"); } - } - - assert(x.size() == y.size()); - assert(y.size() == z.size()); - - PyObject *xarray = detail::get_array(x); - PyObject *yarray = detail::get_array(y); - PyObject *zarray = detail::get_array(z); - - // construct positional args - PyObject *args = PyTuple_New(3); - PyTuple_SetItem(args, 0, xarray); - PyTuple_SetItem(args, 1, yarray); - PyTuple_SetItem(args, 2, zarray); - - // Build up the kw args. - PyObject *kwargs = PyDict_New(); - - for (std::map::const_iterator it = keywords.begin(); - it != keywords.end(); ++it) { - PyDict_SetItemString(kwargs, it->first.c_str(), - PyString_FromString(it->second.c_str())); - } - - PyObject *fig = - PyObject_CallObject(detail::_interpreter::get().s_python_function_figure, - detail::_interpreter::get().s_python_empty_tuple); - if (!fig) throw std::runtime_error("Call to figure() failed."); - - PyObject *gca_kwargs = PyDict_New(); - PyDict_SetItemString(gca_kwargs, "projection", PyString_FromString("3d")); - - PyObject *gca = PyObject_GetAttrString(fig, "gca"); - if (!gca) throw std::runtime_error("No gca"); - Py_INCREF(gca); - PyObject *axis = PyObject_Call( - gca, detail::_interpreter::get().s_python_empty_tuple, gca_kwargs); - - if (!axis) throw std::runtime_error("No axis"); - Py_INCREF(axis); - - Py_DECREF(gca); - Py_DECREF(gca_kwargs); - - PyObject *plot3 = PyObject_GetAttrString(axis, "plot"); - if (!plot3) throw std::runtime_error("No 3D line plot"); - Py_INCREF(plot3); - PyObject *res = PyObject_Call(plot3, args, kwargs); - if (!res) throw std::runtime_error("Failed 3D line plot"); - Py_DECREF(plot3); - - Py_DECREF(axis); - Py_DECREF(args); - Py_DECREF(kwargs); - if (res) Py_DECREF(res); -} - -template -bool stem(const std::vector &x, const std::vector &y, const std::map& keywords) -{ - assert(x.size() == y.size()); - - detail::_interpreter::get(); - - // using numpy arrays - PyObject* xarray = detail::get_array(x); - PyObject* yarray = detail::get_array(y); - - // construct positional args - PyObject* args = PyTuple_New(2); - PyTuple_SetItem(args, 0, xarray); - PyTuple_SetItem(args, 1, yarray); - - // construct keyword args - PyObject* kwargs = PyDict_New(); - for (std::map::const_iterator it = - keywords.begin(); it != keywords.end(); ++it) { - PyDict_SetItemString(kwargs, it->first.c_str(), - PyString_FromString(it->second.c_str())); - } - - PyObject* res = PyObject_Call( - detail::_interpreter::get().s_python_function_stem, args, kwargs); - - Py_DECREF(args); - Py_DECREF(kwargs); - if (res) - Py_DECREF(res); - - return res; -} - -template< typename Numeric > -bool fill(const std::vector& x, const std::vector& y, const std::map& keywords) -{ - assert(x.size() == y.size()); - - detail::_interpreter::get(); - - // using numpy arrays - PyObject* xarray = detail::get_array(x); - PyObject* yarray = detail::get_array(y); - - // construct positional args - PyObject* args = PyTuple_New(2); - PyTuple_SetItem(args, 0, xarray); - PyTuple_SetItem(args, 1, yarray); - - // construct keyword args - PyObject* kwargs = PyDict_New(); - for (auto it = keywords.begin(); it != keywords.end(); ++it) { - PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); - } - - PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_fill, args, kwargs); - - Py_DECREF(args); - Py_DECREF(kwargs); - - if (res) Py_DECREF(res); - - return res; -} - -template< typename Numeric > -bool fill_between(const std::vector& x, const std::vector& y1, const std::vector& y2, const std::map& keywords) -{ - assert(x.size() == y1.size()); - assert(x.size() == y2.size()); - - detail::_interpreter::get(); - - // using numpy arrays - PyObject* xarray = detail::get_array(x); - PyObject* y1array = detail::get_array(y1); - PyObject* y2array = detail::get_array(y2); - - // construct positional args - PyObject* args = PyTuple_New(3); - PyTuple_SetItem(args, 0, xarray); - PyTuple_SetItem(args, 1, y1array); - PyTuple_SetItem(args, 2, y2array); - - // construct keyword args - PyObject* kwargs = PyDict_New(); - for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) { - PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); - } - - PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_fill_between, args, kwargs); - - Py_DECREF(args); - Py_DECREF(kwargs); - if(res) Py_DECREF(res); - - return res; -} - -template -bool arrow(Numeric x, Numeric y, Numeric end_x, Numeric end_y, const std::string& fc = "r", - const std::string ec = "k", Numeric head_length = 0.25, Numeric head_width = 0.1625) { - PyObject* obj_x = PyFloat_FromDouble(x); - PyObject* obj_y = PyFloat_FromDouble(y); - PyObject* obj_end_x = PyFloat_FromDouble(end_x); - PyObject* obj_end_y = PyFloat_FromDouble(end_y); - - PyObject* kwargs = PyDict_New(); - PyDict_SetItemString(kwargs, "fc", PyString_FromString(fc.c_str())); - PyDict_SetItemString(kwargs, "ec", PyString_FromString(ec.c_str())); - PyDict_SetItemString(kwargs, "head_width", PyFloat_FromDouble(head_width)); - PyDict_SetItemString(kwargs, "head_length", PyFloat_FromDouble(head_length)); - - PyObject* plot_args = PyTuple_New(4); - PyTuple_SetItem(plot_args, 0, obj_x); - PyTuple_SetItem(plot_args, 1, obj_y); - PyTuple_SetItem(plot_args, 2, obj_end_x); - PyTuple_SetItem(plot_args, 3, obj_end_y); - - PyObject* res = - PyObject_Call(detail::_interpreter::get().s_python_function_arrow, plot_args, kwargs); - - Py_DECREF(plot_args); - Py_DECREF(kwargs); - if (res) - Py_DECREF(res); - - return res; -} - -template< typename Numeric> -bool hist(const std::vector& y, long bins=10,std::string color="b", - double alpha=1.0, bool cumulative=false) -{ - detail::_interpreter::get(); - - PyObject* yarray = detail::get_array(y); - - PyObject* kwargs = PyDict_New(); - PyDict_SetItemString(kwargs, "bins", PyLong_FromLong(bins)); - PyDict_SetItemString(kwargs, "color", PyString_FromString(color.c_str())); - PyDict_SetItemString(kwargs, "alpha", PyFloat_FromDouble(alpha)); - PyDict_SetItemString(kwargs, "cumulative", cumulative ? Py_True : Py_False); - - PyObject* plot_args = PyTuple_New(1); - - PyTuple_SetItem(plot_args, 0, yarray); - - - PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_hist, plot_args, kwargs); - - - Py_DECREF(plot_args); - Py_DECREF(kwargs); - if(res) Py_DECREF(res); - - return res; -} - -#ifndef WITHOUT_NUMPY -namespace detail { - -inline void imshow(void *ptr, const NPY_TYPES type, const int rows, const int columns, const int colors, const std::map &keywords, PyObject** out) -{ - assert(type == NPY_UINT8 || type == NPY_FLOAT); - assert(colors == 1 || colors == 3 || colors == 4); - - detail::_interpreter::get(); - - // construct args - npy_intp dims[3] = { rows, columns, colors }; - PyObject *args = PyTuple_New(1); - PyTuple_SetItem(args, 0, PyArray_SimpleNewFromData(colors == 1 ? 2 : 3, dims, type, ptr)); - - // construct keyword args - PyObject* kwargs = PyDict_New(); - for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) - { - PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); - } - - PyObject *res = PyObject_Call(detail::_interpreter::get().s_python_function_imshow, args, kwargs); - Py_DECREF(args); - Py_DECREF(kwargs); - if (!res) - throw std::runtime_error("Call to imshow() failed"); - if (out) - *out = res; - else - Py_DECREF(res); -} - -} // namespace detail - -inline void imshow(const unsigned char *ptr, const int rows, const int columns, const int colors, const std::map &keywords = {}, PyObject** out = nullptr) -{ - detail::imshow((void *) ptr, NPY_UINT8, rows, columns, colors, keywords, out); -} - -inline void imshow(const float *ptr, const int rows, const int columns, const int colors, const std::map &keywords = {}, PyObject** out = nullptr) -{ - detail::imshow((void *) ptr, NPY_FLOAT, rows, columns, colors, keywords, out); -} - -#ifdef WITH_OPENCV -void imshow(const cv::Mat &image, const std::map &keywords = {}) -{ - // Convert underlying type of matrix, if needed - cv::Mat image2; - NPY_TYPES npy_type = NPY_UINT8; - switch (image.type() & CV_MAT_DEPTH_MASK) { - case CV_8U: - image2 = image; - break; - case CV_32F: - image2 = image; - npy_type = NPY_FLOAT; - break; - default: - image.convertTo(image2, CV_MAKETYPE(CV_8U, image.channels())); - } - - // If color image, convert from BGR to RGB - switch (image2.channels()) { - case 3: - cv::cvtColor(image2, image2, CV_BGR2RGB); - break; - case 4: - cv::cvtColor(image2, image2, CV_BGRA2RGBA); - } - - detail::imshow(image2.data, npy_type, image2.rows, image2.cols, image2.channels(), keywords); -} -#endif // WITH_OPENCV -#endif // WITHOUT_NUMPY - -template -bool scatter(const std::vector& x, - const std::vector& y, - const double s=1.0, // The marker size in points**2 - const std::map & keywords = {}) -{ - detail::_interpreter::get(); - - assert(x.size() == y.size()); - - PyObject* xarray = detail::get_array(x); - PyObject* yarray = detail::get_array(y); - - PyObject* kwargs = PyDict_New(); - PyDict_SetItemString(kwargs, "s", PyLong_FromLong(s)); - for (const auto& it : keywords) - { - PyDict_SetItemString(kwargs, it.first.c_str(), PyString_FromString(it.second.c_str())); - } - - PyObject* plot_args = PyTuple_New(2); - PyTuple_SetItem(plot_args, 0, xarray); - PyTuple_SetItem(plot_args, 1, yarray); - - PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_scatter, plot_args, kwargs); - - Py_DECREF(plot_args); - Py_DECREF(kwargs); - if(res) Py_DECREF(res); - - return res; -} - -template -bool boxplot(const std::vector>& data, - const std::vector& labels = {}, - const std::map & keywords = {}) -{ - detail::_interpreter::get(); - - PyObject* listlist = detail::get_listlist(data); - PyObject* args = PyTuple_New(1); - PyTuple_SetItem(args, 0, listlist); - - PyObject* kwargs = PyDict_New(); - - // kwargs needs the labels, if there are (the correct number of) labels - if (!labels.empty() && labels.size() == data.size()) { - PyDict_SetItemString(kwargs, "labels", detail::get_array(labels)); - } - - // take care of the remaining keywords - for (const auto& it : keywords) - { - PyDict_SetItemString(kwargs, it.first.c_str(), PyString_FromString(it.second.c_str())); - } - - PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_boxplot, args, kwargs); - - Py_DECREF(args); - Py_DECREF(kwargs); - - if(res) Py_DECREF(res); - - return res; -} - -template -bool boxplot(const std::vector& data, - const std::map & keywords = {}) -{ - detail::_interpreter::get(); - - PyObject* vector = detail::get_array(data); - PyObject* args = PyTuple_New(1); - PyTuple_SetItem(args, 0, vector); - - PyObject* kwargs = PyDict_New(); - for (const auto& it : keywords) - { - PyDict_SetItemString(kwargs, it.first.c_str(), PyString_FromString(it.second.c_str())); - } - - PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_boxplot, args, kwargs); - - Py_DECREF(args); - Py_DECREF(kwargs); - - if(res) Py_DECREF(res); - - return res; -} - -template -bool bar(const std::vector & x, - const std::vector & y, - std::string ec = "black", - std::string ls = "-", - double lw = 1.0, - const std::map & keywords = {}) -{ - detail::_interpreter::get(); - - PyObject * xarray = detail::get_array(x); - PyObject * yarray = detail::get_array(y); - - PyObject * kwargs = PyDict_New(); - - PyDict_SetItemString(kwargs, "ec", PyString_FromString(ec.c_str())); - PyDict_SetItemString(kwargs, "ls", PyString_FromString(ls.c_str())); - PyDict_SetItemString(kwargs, "lw", PyFloat_FromDouble(lw)); - - for (std::map::const_iterator it = - keywords.begin(); - it != keywords.end(); - ++it) { - PyDict_SetItemString( - kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); - } - - PyObject * plot_args = PyTuple_New(2); - PyTuple_SetItem(plot_args, 0, xarray); - PyTuple_SetItem(plot_args, 1, yarray); - - PyObject * res = PyObject_Call( - detail::_interpreter::get().s_python_function_bar, plot_args, kwargs); - - Py_DECREF(plot_args); - Py_DECREF(kwargs); - if (res) Py_DECREF(res); - - return res; -} - -template -bool bar(const std::vector & y, - std::string ec = "black", - std::string ls = "-", - double lw = 1.0, - const std::map & keywords = {}) -{ - using T = typename std::remove_reference::type::value_type; - - detail::_interpreter::get(); - - std::vector x; - for (std::size_t i = 0; i < y.size(); i++) { x.push_back(i); } - - return bar(x, y, ec, ls, lw, keywords); -} - - -template -bool barh(const std::vector &x, const std::vector &y, std::string ec = "black", std::string ls = "-", double lw = 1.0, const std::map &keywords = { }) { - PyObject *xarray = detail::get_array(x); - PyObject *yarray = detail::get_array(y); - - PyObject *kwargs = PyDict_New(); - - PyDict_SetItemString(kwargs, "ec", PyString_FromString(ec.c_str())); - PyDict_SetItemString(kwargs, "ls", PyString_FromString(ls.c_str())); - PyDict_SetItemString(kwargs, "lw", PyFloat_FromDouble(lw)); - - for (std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) { - PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); - } - - PyObject *plot_args = PyTuple_New(2); - PyTuple_SetItem(plot_args, 0, xarray); - PyTuple_SetItem(plot_args, 1, yarray); - - PyObject *res = PyObject_Call(detail::_interpreter::get().s_python_function_barh, plot_args, kwargs); - - Py_DECREF(plot_args); - Py_DECREF(kwargs); - if (res) Py_DECREF(res); - - return res; -} - - -inline bool subplots_adjust(const std::map& keywords = {}) -{ - detail::_interpreter::get(); - - PyObject* kwargs = PyDict_New(); - for (std::map::const_iterator it = - keywords.begin(); it != keywords.end(); ++it) { - PyDict_SetItemString(kwargs, it->first.c_str(), - PyFloat_FromDouble(it->second)); - } - - - PyObject* plot_args = PyTuple_New(0); - - PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_subplots_adjust, plot_args, kwargs); - - Py_DECREF(plot_args); - Py_DECREF(kwargs); - if(res) Py_DECREF(res); - - return res; -} - -template< typename Numeric> -bool named_hist(std::string label,const std::vector& y, long bins=10, std::string color="b", double alpha=1.0) -{ - detail::_interpreter::get(); - - PyObject* yarray = detail::get_array(y); - - PyObject* kwargs = PyDict_New(); - PyDict_SetItemString(kwargs, "label", PyString_FromString(label.c_str())); - PyDict_SetItemString(kwargs, "bins", PyLong_FromLong(bins)); - PyDict_SetItemString(kwargs, "color", PyString_FromString(color.c_str())); - PyDict_SetItemString(kwargs, "alpha", PyFloat_FromDouble(alpha)); - - - PyObject* plot_args = PyTuple_New(1); - PyTuple_SetItem(plot_args, 0, yarray); - - PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_hist, plot_args, kwargs); - - Py_DECREF(plot_args); - Py_DECREF(kwargs); - if(res) Py_DECREF(res); - - return res; -} - -template -bool plot(const std::vector& x, const std::vector& y, const std::string& s = "") -{ - assert(x.size() == y.size()); - - detail::_interpreter::get(); - - PyObject* xarray = detail::get_array(x); - PyObject* yarray = detail::get_array(y); - - PyObject* pystring = PyString_FromString(s.c_str()); - - PyObject* plot_args = PyTuple_New(3); - PyTuple_SetItem(plot_args, 0, xarray); - PyTuple_SetItem(plot_args, 1, yarray); - PyTuple_SetItem(plot_args, 2, pystring); - - PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_plot, plot_args); - - Py_DECREF(plot_args); - if(res) Py_DECREF(res); - - return res; -} - -template -bool contour(const std::vector& x, const std::vector& y, - const std::vector& z, - const std::map& keywords = {}) { - assert(x.size() == y.size() && x.size() == z.size()); - - PyObject* xarray = get_array(x); - PyObject* yarray = get_array(y); - PyObject* zarray = get_array(z); - - PyObject* plot_args = PyTuple_New(3); - PyTuple_SetItem(plot_args, 0, xarray); - PyTuple_SetItem(plot_args, 1, yarray); - PyTuple_SetItem(plot_args, 2, zarray); - - // construct keyword args - PyObject* kwargs = PyDict_New(); - for (std::map::const_iterator it = keywords.begin(); - it != keywords.end(); ++it) { - PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); - } - - PyObject* res = - PyObject_Call(detail::_interpreter::get().s_python_function_contour, plot_args, kwargs); - - Py_DECREF(kwargs); - Py_DECREF(plot_args); - if (res) - Py_DECREF(res); - - return res; -} - -template -bool quiver(const std::vector& x, const std::vector& y, const std::vector& u, const std::vector& w, const std::map& keywords = {}) -{ - assert(x.size() == y.size() && x.size() == u.size() && u.size() == w.size()); - - detail::_interpreter::get(); - - PyObject* xarray = detail::get_array(x); - PyObject* yarray = detail::get_array(y); - PyObject* uarray = detail::get_array(u); - PyObject* warray = detail::get_array(w); - - PyObject* plot_args = PyTuple_New(4); - PyTuple_SetItem(plot_args, 0, xarray); - PyTuple_SetItem(plot_args, 1, yarray); - PyTuple_SetItem(plot_args, 2, uarray); - PyTuple_SetItem(plot_args, 3, warray); - - // construct keyword args - PyObject* kwargs = PyDict_New(); - for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) - { - PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); - } - - PyObject* res = PyObject_Call( - detail::_interpreter::get().s_python_function_quiver, plot_args, kwargs); - - Py_DECREF(kwargs); - Py_DECREF(plot_args); - if (res) - Py_DECREF(res); - - return res; -} - -template -bool stem(const std::vector& x, const std::vector& y, const std::string& s = "") -{ - assert(x.size() == y.size()); - - detail::_interpreter::get(); - - PyObject* xarray = detail::get_array(x); - PyObject* yarray = detail::get_array(y); - - PyObject* pystring = PyString_FromString(s.c_str()); - - PyObject* plot_args = PyTuple_New(3); - PyTuple_SetItem(plot_args, 0, xarray); - PyTuple_SetItem(plot_args, 1, yarray); - PyTuple_SetItem(plot_args, 2, pystring); - - PyObject* res = PyObject_CallObject( - detail::_interpreter::get().s_python_function_stem, plot_args); - - Py_DECREF(plot_args); - if (res) - Py_DECREF(res); - - return res; -} - -template -bool semilogx(const std::vector& x, const std::vector& y, const std::string& s = "") -{ - assert(x.size() == y.size()); - - detail::_interpreter::get(); - - PyObject* xarray = detail::get_array(x); - PyObject* yarray = detail::get_array(y); - - PyObject* pystring = PyString_FromString(s.c_str()); - - PyObject* plot_args = PyTuple_New(3); - PyTuple_SetItem(plot_args, 0, xarray); - PyTuple_SetItem(plot_args, 1, yarray); - PyTuple_SetItem(plot_args, 2, pystring); - - PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_semilogx, plot_args); - - Py_DECREF(plot_args); - if(res) Py_DECREF(res); - - return res; -} - -template -bool semilogy(const std::vector& x, const std::vector& y, const std::string& s = "") -{ - assert(x.size() == y.size()); - - detail::_interpreter::get(); - - PyObject* xarray = detail::get_array(x); - PyObject* yarray = detail::get_array(y); - - PyObject* pystring = PyString_FromString(s.c_str()); - - PyObject* plot_args = PyTuple_New(3); - PyTuple_SetItem(plot_args, 0, xarray); - PyTuple_SetItem(plot_args, 1, yarray); - PyTuple_SetItem(plot_args, 2, pystring); - - PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_semilogy, plot_args); - - Py_DECREF(plot_args); - if(res) Py_DECREF(res); - - return res; -} - -template -bool loglog(const std::vector& x, const std::vector& y, const std::string& s = "") -{ - assert(x.size() == y.size()); - - detail::_interpreter::get(); - - PyObject* xarray = detail::get_array(x); - PyObject* yarray = detail::get_array(y); - - PyObject* pystring = PyString_FromString(s.c_str()); - - PyObject* plot_args = PyTuple_New(3); - PyTuple_SetItem(plot_args, 0, xarray); - PyTuple_SetItem(plot_args, 1, yarray); - PyTuple_SetItem(plot_args, 2, pystring); - - PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_loglog, plot_args); - - Py_DECREF(plot_args); - if(res) Py_DECREF(res); - - return res; -} - -template -bool errorbar(const std::vector &x, const std::vector &y, const std::vector &yerr, const std::map &keywords = {}) -{ - assert(x.size() == y.size()); - - detail::_interpreter::get(); - - PyObject* xarray = detail::get_array(x); - PyObject* yarray = detail::get_array(y); - PyObject* yerrarray = detail::get_array(yerr); - - // construct keyword args - PyObject* kwargs = PyDict_New(); - for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) - { - PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str())); - } - - PyDict_SetItemString(kwargs, "yerr", yerrarray); - - PyObject *plot_args = PyTuple_New(2); - PyTuple_SetItem(plot_args, 0, xarray); - PyTuple_SetItem(plot_args, 1, yarray); - - PyObject *res = PyObject_Call(detail::_interpreter::get().s_python_function_errorbar, plot_args, kwargs); - - Py_DECREF(kwargs); - Py_DECREF(plot_args); - - if (res) - Py_DECREF(res); - else - throw std::runtime_error("Call to errorbar() failed."); - - return res; -} - -template -bool named_plot(const std::string& name, const std::vector& y, const std::string& format = "") -{ - detail::_interpreter::get(); - - PyObject* kwargs = PyDict_New(); - PyDict_SetItemString(kwargs, "label", PyString_FromString(name.c_str())); - - PyObject* yarray = detail::get_array(y); - - PyObject* pystring = PyString_FromString(format.c_str()); - - PyObject* plot_args = PyTuple_New(2); - - PyTuple_SetItem(plot_args, 0, yarray); - PyTuple_SetItem(plot_args, 1, pystring); - - PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_plot, plot_args, kwargs); - - Py_DECREF(kwargs); - Py_DECREF(plot_args); - if (res) Py_DECREF(res); - - return res; -} - -template -bool named_plot(const std::string& name, const std::vector& x, const std::vector& y, const std::string& format = "") -{ - detail::_interpreter::get(); - - PyObject* kwargs = PyDict_New(); - PyDict_SetItemString(kwargs, "label", PyString_FromString(name.c_str())); - - PyObject* xarray = detail::get_array(x); - PyObject* yarray = detail::get_array(y); - - PyObject* pystring = PyString_FromString(format.c_str()); - - PyObject* plot_args = PyTuple_New(3); - PyTuple_SetItem(plot_args, 0, xarray); - PyTuple_SetItem(plot_args, 1, yarray); - PyTuple_SetItem(plot_args, 2, pystring); - - PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_plot, plot_args, kwargs); - - Py_DECREF(kwargs); - Py_DECREF(plot_args); - if (res) Py_DECREF(res); - - return res; -} - -template -bool named_semilogx(const std::string& name, const std::vector& x, const std::vector& y, const std::string& format = "") -{ - detail::_interpreter::get(); - - PyObject* kwargs = PyDict_New(); - PyDict_SetItemString(kwargs, "label", PyString_FromString(name.c_str())); - - PyObject* xarray = detail::get_array(x); - PyObject* yarray = detail::get_array(y); - - PyObject* pystring = PyString_FromString(format.c_str()); - - PyObject* plot_args = PyTuple_New(3); - PyTuple_SetItem(plot_args, 0, xarray); - PyTuple_SetItem(plot_args, 1, yarray); - PyTuple_SetItem(plot_args, 2, pystring); - - PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_semilogx, plot_args, kwargs); - - Py_DECREF(kwargs); - Py_DECREF(plot_args); - if (res) Py_DECREF(res); - - return res; -} - -template -bool named_semilogy(const std::string& name, const std::vector& x, const std::vector& y, const std::string& format = "") -{ - detail::_interpreter::get(); - - PyObject* kwargs = PyDict_New(); - PyDict_SetItemString(kwargs, "label", PyString_FromString(name.c_str())); - - PyObject* xarray = detail::get_array(x); - PyObject* yarray = detail::get_array(y); - - PyObject* pystring = PyString_FromString(format.c_str()); - - PyObject* plot_args = PyTuple_New(3); - PyTuple_SetItem(plot_args, 0, xarray); - PyTuple_SetItem(plot_args, 1, yarray); - PyTuple_SetItem(plot_args, 2, pystring); - - PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_semilogy, plot_args, kwargs); - - Py_DECREF(kwargs); - Py_DECREF(plot_args); - if (res) Py_DECREF(res); - - return res; -} - -template -bool named_loglog(const std::string& name, const std::vector& x, const std::vector& y, const std::string& format = "") -{ - detail::_interpreter::get(); - - PyObject* kwargs = PyDict_New(); - PyDict_SetItemString(kwargs, "label", PyString_FromString(name.c_str())); - - PyObject* xarray = detail::get_array(x); - PyObject* yarray = detail::get_array(y); - - PyObject* pystring = PyString_FromString(format.c_str()); - - PyObject* plot_args = PyTuple_New(3); - PyTuple_SetItem(plot_args, 0, xarray); - PyTuple_SetItem(plot_args, 1, yarray); - PyTuple_SetItem(plot_args, 2, pystring); - PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_loglog, plot_args, kwargs); - - Py_DECREF(kwargs); - Py_DECREF(plot_args); - if (res) Py_DECREF(res); - - return res; -} - -template -bool plot(const std::vector& y, const std::string& format = "") -{ - std::vector x(y.size()); - for(size_t i=0; i -bool plot(const std::vector& y, const std::map& keywords) -{ - std::vector x(y.size()); - for(size_t i=0; i -bool stem(const std::vector& y, const std::string& format = "") -{ - std::vector x(y.size()); - for (size_t i = 0; i < x.size(); ++i) x.at(i) = i; - return stem(x, y, format); -} - -template -void text(Numeric x, Numeric y, const std::string& s = "") -{ - detail::_interpreter::get(); - - PyObject* args = PyTuple_New(3); - PyTuple_SetItem(args, 0, PyFloat_FromDouble(x)); - PyTuple_SetItem(args, 1, PyFloat_FromDouble(y)); - PyTuple_SetItem(args, 2, PyString_FromString(s.c_str())); - - PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_text, args); - if(!res) throw std::runtime_error("Call to text() failed."); - - Py_DECREF(args); - Py_DECREF(res); -} - -inline void colorbar(PyObject* mappable = NULL, const std::map& keywords = {}) -{ - if (mappable == NULL) - throw std::runtime_error("Must call colorbar with PyObject* returned from an image, contour, surface, etc."); - - detail::_interpreter::get(); - - PyObject* args = PyTuple_New(1); - PyTuple_SetItem(args, 0, mappable); - - PyObject* kwargs = PyDict_New(); - for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) - { - PyDict_SetItemString(kwargs, it->first.c_str(), PyFloat_FromDouble(it->second)); - } - - PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_colorbar, args, kwargs); - if(!res) throw std::runtime_error("Call to colorbar() failed."); - - Py_DECREF(args); - Py_DECREF(kwargs); - Py_DECREF(res); -} - - -inline long figure(long number = -1) -{ - detail::_interpreter::get(); - - PyObject *res; - if (number == -1) - res = PyObject_CallObject(detail::_interpreter::get().s_python_function_figure, detail::_interpreter::get().s_python_empty_tuple); - else { - assert(number > 0); - - // Make sure interpreter is initialised - detail::_interpreter::get(); - - PyObject *args = PyTuple_New(1); - PyTuple_SetItem(args, 0, PyLong_FromLong(number)); - res = PyObject_CallObject(detail::_interpreter::get().s_python_function_figure, args); - Py_DECREF(args); - } - - if(!res) throw std::runtime_error("Call to figure() failed."); - - PyObject* num = PyObject_GetAttrString(res, "number"); - if (!num) throw std::runtime_error("Could not get number attribute of figure object"); - const long figureNumber = PyLong_AsLong(num); - - Py_DECREF(num); - Py_DECREF(res); - - return figureNumber; -} - -inline bool fignum_exists(long number) -{ - detail::_interpreter::get(); - - PyObject *args = PyTuple_New(1); - PyTuple_SetItem(args, 0, PyLong_FromLong(number)); - PyObject *res = PyObject_CallObject(detail::_interpreter::get().s_python_function_fignum_exists, args); - if(!res) throw std::runtime_error("Call to fignum_exists() failed."); - - bool ret = PyObject_IsTrue(res); - Py_DECREF(res); - Py_DECREF(args); - - return ret; -} - -inline void figure_size(size_t w, size_t h) -{ - detail::_interpreter::get(); - - const size_t dpi = 100; - PyObject* size = PyTuple_New(2); - PyTuple_SetItem(size, 0, PyFloat_FromDouble((double)w / dpi)); - PyTuple_SetItem(size, 1, PyFloat_FromDouble((double)h / dpi)); - - PyObject* kwargs = PyDict_New(); - PyDict_SetItemString(kwargs, "figsize", size); - PyDict_SetItemString(kwargs, "dpi", PyLong_FromSize_t(dpi)); - - PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_figure, - detail::_interpreter::get().s_python_empty_tuple, kwargs); - - Py_DECREF(kwargs); - - if(!res) throw std::runtime_error("Call to figure_size() failed."); - Py_DECREF(res); -} - -inline void legend() -{ - detail::_interpreter::get(); - - PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_legend, detail::_interpreter::get().s_python_empty_tuple); - if(!res) throw std::runtime_error("Call to legend() failed."); - - Py_DECREF(res); -} - -inline void legend(const std::map& keywords) -{ - detail::_interpreter::get(); - - // construct keyword args - PyObject* kwargs = PyDict_New(); - for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) - { - PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str())); - } - - PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_legend, detail::_interpreter::get().s_python_empty_tuple, kwargs); - if(!res) throw std::runtime_error("Call to legend() failed."); - - Py_DECREF(kwargs); - Py_DECREF(res); -} - -template -void ylim(Numeric left, Numeric right) -{ - detail::_interpreter::get(); - - PyObject* list = PyList_New(2); - PyList_SetItem(list, 0, PyFloat_FromDouble(left)); - PyList_SetItem(list, 1, PyFloat_FromDouble(right)); - - PyObject* args = PyTuple_New(1); - PyTuple_SetItem(args, 0, list); - - PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_ylim, args); - if(!res) throw std::runtime_error("Call to ylim() failed."); - - Py_DECREF(args); - Py_DECREF(res); -} - -template -void xlim(Numeric left, Numeric right) -{ - detail::_interpreter::get(); - - PyObject* list = PyList_New(2); - PyList_SetItem(list, 0, PyFloat_FromDouble(left)); - PyList_SetItem(list, 1, PyFloat_FromDouble(right)); - - PyObject* args = PyTuple_New(1); - PyTuple_SetItem(args, 0, list); - - PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_xlim, args); - if(!res) throw std::runtime_error("Call to xlim() failed."); - - Py_DECREF(args); - Py_DECREF(res); -} - - -inline double* xlim() -{ - detail::_interpreter::get(); - - PyObject* args = PyTuple_New(0); - PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_xlim, args); - PyObject* left = PyTuple_GetItem(res,0); - PyObject* right = PyTuple_GetItem(res,1); - - double* arr = new double[2]; - arr[0] = PyFloat_AsDouble(left); - arr[1] = PyFloat_AsDouble(right); - - if(!res) throw std::runtime_error("Call to xlim() failed."); - - Py_DECREF(res); - return arr; -} - - -inline double* ylim() -{ - detail::_interpreter::get(); - - PyObject* args = PyTuple_New(0); - PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_ylim, args); - PyObject* left = PyTuple_GetItem(res,0); - PyObject* right = PyTuple_GetItem(res,1); - - double* arr = new double[2]; - arr[0] = PyFloat_AsDouble(left); - arr[1] = PyFloat_AsDouble(right); - - if(!res) throw std::runtime_error("Call to ylim() failed."); - - Py_DECREF(res); - return arr; -} - -template -inline void xticks(const std::vector &ticks, const std::vector &labels = {}, const std::map& keywords = {}) -{ - assert(labels.size() == 0 || ticks.size() == labels.size()); - - detail::_interpreter::get(); - - // using numpy array - PyObject* ticksarray = detail::get_array(ticks); - - PyObject* args; - if(labels.size() == 0) { - // construct positional args - args = PyTuple_New(1); - PyTuple_SetItem(args, 0, ticksarray); - } else { - // make tuple of tick labels - PyObject* labelstuple = PyTuple_New(labels.size()); - for (size_t i = 0; i < labels.size(); i++) - PyTuple_SetItem(labelstuple, i, PyUnicode_FromString(labels[i].c_str())); - - // construct positional args - args = PyTuple_New(2); - PyTuple_SetItem(args, 0, ticksarray); - PyTuple_SetItem(args, 1, labelstuple); - } - - // construct keyword args - PyObject* kwargs = PyDict_New(); - for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) - { - PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str())); - } - - PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_xticks, args, kwargs); - - Py_DECREF(args); - Py_DECREF(kwargs); - if(!res) throw std::runtime_error("Call to xticks() failed"); - - Py_DECREF(res); -} - -template -inline void xticks(const std::vector &ticks, const std::map& keywords) -{ - xticks(ticks, {}, keywords); -} - -template -inline void yticks(const std::vector &ticks, const std::vector &labels = {}, const std::map& keywords = {}) -{ - assert(labels.size() == 0 || ticks.size() == labels.size()); - - detail::_interpreter::get(); - - // using numpy array - PyObject* ticksarray = detail::get_array(ticks); - - PyObject* args; - if(labels.size() == 0) { - // construct positional args - args = PyTuple_New(1); - PyTuple_SetItem(args, 0, ticksarray); - } else { - // make tuple of tick labels - PyObject* labelstuple = PyTuple_New(labels.size()); - for (size_t i = 0; i < labels.size(); i++) - PyTuple_SetItem(labelstuple, i, PyUnicode_FromString(labels[i].c_str())); - - // construct positional args - args = PyTuple_New(2); - PyTuple_SetItem(args, 0, ticksarray); - PyTuple_SetItem(args, 1, labelstuple); - } - - // construct keyword args - PyObject* kwargs = PyDict_New(); - for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) - { - PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str())); - } - - PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_yticks, args, kwargs); - - Py_DECREF(args); - Py_DECREF(kwargs); - if(!res) throw std::runtime_error("Call to yticks() failed"); - - Py_DECREF(res); -} - -template -inline void yticks(const std::vector &ticks, const std::map& keywords) -{ - yticks(ticks, {}, keywords); -} - -template inline void margins(Numeric margin) -{ - // construct positional args - PyObject* args = PyTuple_New(1); - PyTuple_SetItem(args, 0, PyFloat_FromDouble(margin)); - - PyObject* res = - PyObject_CallObject(detail::_interpreter::get().s_python_function_margins, args); - if (!res) - throw std::runtime_error("Call to margins() failed."); - - Py_DECREF(args); - Py_DECREF(res); -} - -template inline void margins(Numeric margin_x, Numeric margin_y) -{ - // construct positional args - PyObject* args = PyTuple_New(2); - PyTuple_SetItem(args, 0, PyFloat_FromDouble(margin_x)); - PyTuple_SetItem(args, 1, PyFloat_FromDouble(margin_y)); - - PyObject* res = - PyObject_CallObject(detail::_interpreter::get().s_python_function_margins, args); - if (!res) - throw std::runtime_error("Call to margins() failed."); - - Py_DECREF(args); - Py_DECREF(res); -} - - -inline void tick_params(const std::map& keywords, const std::string axis = "both") -{ - detail::_interpreter::get(); - - // construct positional args - PyObject* args; - args = PyTuple_New(1); - PyTuple_SetItem(args, 0, PyString_FromString(axis.c_str())); - - // construct keyword args - PyObject* kwargs = PyDict_New(); - for (std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) - { - PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str())); - } - - - PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_tick_params, args, kwargs); - - Py_DECREF(args); - Py_DECREF(kwargs); - if (!res) throw std::runtime_error("Call to tick_params() failed"); - - Py_DECREF(res); -} - -inline void subplot(long nrows, long ncols, long plot_number) -{ - detail::_interpreter::get(); - - // construct positional args - PyObject* args = PyTuple_New(3); - PyTuple_SetItem(args, 0, PyFloat_FromDouble(nrows)); - PyTuple_SetItem(args, 1, PyFloat_FromDouble(ncols)); - PyTuple_SetItem(args, 2, PyFloat_FromDouble(plot_number)); - - PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_subplot, args); - if(!res) throw std::runtime_error("Call to subplot() failed."); - - Py_DECREF(args); - Py_DECREF(res); -} - -inline void subplot2grid(long nrows, long ncols, long rowid=0, long colid=0, long rowspan=1, long colspan=1) -{ - detail::_interpreter::get(); - - PyObject* shape = PyTuple_New(2); - PyTuple_SetItem(shape, 0, PyLong_FromLong(nrows)); - PyTuple_SetItem(shape, 1, PyLong_FromLong(ncols)); - - PyObject* loc = PyTuple_New(2); - PyTuple_SetItem(loc, 0, PyLong_FromLong(rowid)); - PyTuple_SetItem(loc, 1, PyLong_FromLong(colid)); - - PyObject* args = PyTuple_New(4); - PyTuple_SetItem(args, 0, shape); - PyTuple_SetItem(args, 1, loc); - PyTuple_SetItem(args, 2, PyLong_FromLong(rowspan)); - PyTuple_SetItem(args, 3, PyLong_FromLong(colspan)); - - PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_subplot2grid, args); - if(!res) throw std::runtime_error("Call to subplot2grid() failed."); - - Py_DECREF(shape); - Py_DECREF(loc); - Py_DECREF(args); - Py_DECREF(res); -} - -inline void title(const std::string &titlestr, const std::map &keywords = {}) -{ - detail::_interpreter::get(); - - PyObject* pytitlestr = PyString_FromString(titlestr.c_str()); - PyObject* args = PyTuple_New(1); - PyTuple_SetItem(args, 0, pytitlestr); - - PyObject* kwargs = PyDict_New(); - for (auto it = keywords.begin(); it != keywords.end(); ++it) { - PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); - } - - PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_title, args, kwargs); - if(!res) throw std::runtime_error("Call to title() failed."); - - Py_DECREF(args); - Py_DECREF(kwargs); - Py_DECREF(res); -} - -inline void suptitle(const std::string &suptitlestr, const std::map &keywords = {}) -{ - detail::_interpreter::get(); - - PyObject* pysuptitlestr = PyString_FromString(suptitlestr.c_str()); - PyObject* args = PyTuple_New(1); - PyTuple_SetItem(args, 0, pysuptitlestr); - - PyObject* kwargs = PyDict_New(); - for (auto it = keywords.begin(); it != keywords.end(); ++it) { - PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); - } - - PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_suptitle, args, kwargs); - if(!res) throw std::runtime_error("Call to suptitle() failed."); - - Py_DECREF(args); - Py_DECREF(kwargs); - Py_DECREF(res); -} - -inline void axis(const std::string &axisstr) -{ - detail::_interpreter::get(); - - PyObject* str = PyString_FromString(axisstr.c_str()); - PyObject* args = PyTuple_New(1); - PyTuple_SetItem(args, 0, str); - - PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_axis, args); - if(!res) throw std::runtime_error("Call to title() failed."); - - Py_DECREF(args); - Py_DECREF(res); -} - -inline void axvline(double x, double ymin = 0., double ymax = 1., const std::map& keywords = std::map()) -{ - detail::_interpreter::get(); - - // construct positional args - PyObject* args = PyTuple_New(3); - PyTuple_SetItem(args, 0, PyFloat_FromDouble(x)); - PyTuple_SetItem(args, 1, PyFloat_FromDouble(ymin)); - PyTuple_SetItem(args, 2, PyFloat_FromDouble(ymax)); - - // construct keyword args - PyObject* kwargs = PyDict_New(); - for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) - { - PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str())); - } - - PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_axvline, args, kwargs); - - Py_DECREF(args); - Py_DECREF(kwargs); - - if(res) Py_DECREF(res); -} - -inline void axvspan(double xmin, double xmax, double ymin = 0., double ymax = 1., const std::map& keywords = std::map()) -{ - // construct positional args - PyObject* args = PyTuple_New(4); - PyTuple_SetItem(args, 0, PyFloat_FromDouble(xmin)); - PyTuple_SetItem(args, 1, PyFloat_FromDouble(xmax)); - PyTuple_SetItem(args, 2, PyFloat_FromDouble(ymin)); - PyTuple_SetItem(args, 3, PyFloat_FromDouble(ymax)); - - // construct keyword args - PyObject* kwargs = PyDict_New(); - for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) - { - if (it->first == "linewidth" || it->first == "alpha") - PyDict_SetItemString(kwargs, it->first.c_str(), PyFloat_FromDouble(std::stod(it->second))); - else - PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str())); - } - - PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_axvspan, args, kwargs); - Py_DECREF(args); - Py_DECREF(kwargs); - - if(res) Py_DECREF(res); -} - -inline void xlabel(const std::string &str, const std::map &keywords = {}) -{ - detail::_interpreter::get(); - - PyObject* pystr = PyString_FromString(str.c_str()); - PyObject* args = PyTuple_New(1); - PyTuple_SetItem(args, 0, pystr); - - PyObject* kwargs = PyDict_New(); - for (auto it = keywords.begin(); it != keywords.end(); ++it) { - PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); - } - - PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_xlabel, args, kwargs); - if(!res) throw std::runtime_error("Call to xlabel() failed."); - - Py_DECREF(args); - Py_DECREF(kwargs); - Py_DECREF(res); -} - -inline void ylabel(const std::string &str, const std::map& keywords = {}) -{ - detail::_interpreter::get(); - - PyObject* pystr = PyString_FromString(str.c_str()); - PyObject* args = PyTuple_New(1); - PyTuple_SetItem(args, 0, pystr); - - PyObject* kwargs = PyDict_New(); - for (auto it = keywords.begin(); it != keywords.end(); ++it) { - PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); - } - - PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_ylabel, args, kwargs); - if(!res) throw std::runtime_error("Call to ylabel() failed."); - - Py_DECREF(args); - Py_DECREF(kwargs); - Py_DECREF(res); -} - -inline void set_zlabel(const std::string &str, const std::map& keywords = {}) -{ - detail::_interpreter::get(); - - // Same as with plot_surface: We lazily load the modules here the first time - // this function is called because I'm not sure that we can assume "matplotlib - // installed" implies "mpl_toolkits installed" on all platforms, and we don't - // want to require it for people who don't need 3d plots. - static PyObject *mpl_toolkitsmod = nullptr, *axis3dmod = nullptr; - if (!mpl_toolkitsmod) { - PyObject* mpl_toolkits = PyString_FromString("mpl_toolkits"); - PyObject* axis3d = PyString_FromString("mpl_toolkits.mplot3d"); - if (!mpl_toolkits || !axis3d) { throw std::runtime_error("couldnt create string"); } - - mpl_toolkitsmod = PyImport_Import(mpl_toolkits); - Py_DECREF(mpl_toolkits); - if (!mpl_toolkitsmod) { throw std::runtime_error("Error loading module mpl_toolkits!"); } - - axis3dmod = PyImport_Import(axis3d); - Py_DECREF(axis3d); - if (!axis3dmod) { throw std::runtime_error("Error loading module mpl_toolkits.mplot3d!"); } - } - - PyObject* pystr = PyString_FromString(str.c_str()); - PyObject* args = PyTuple_New(1); - PyTuple_SetItem(args, 0, pystr); - - PyObject* kwargs = PyDict_New(); - for (auto it = keywords.begin(); it != keywords.end(); ++it) { - PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); - } - - PyObject *ax = - PyObject_CallObject(detail::_interpreter::get().s_python_function_gca, - detail::_interpreter::get().s_python_empty_tuple); - if (!ax) throw std::runtime_error("Call to gca() failed."); - Py_INCREF(ax); - - PyObject *zlabel = PyObject_GetAttrString(ax, "set_zlabel"); - if (!zlabel) throw std::runtime_error("Attribute set_zlabel not found."); - Py_INCREF(zlabel); - - PyObject *res = PyObject_Call(zlabel, args, kwargs); - if (!res) throw std::runtime_error("Call to set_zlabel() failed."); - Py_DECREF(zlabel); - - Py_DECREF(ax); - Py_DECREF(args); - Py_DECREF(kwargs); - if (res) Py_DECREF(res); -} - -inline void grid(bool flag) -{ - detail::_interpreter::get(); - - PyObject* pyflag = flag ? Py_True : Py_False; - Py_INCREF(pyflag); - - PyObject* args = PyTuple_New(1); - PyTuple_SetItem(args, 0, pyflag); - - PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_grid, args); - if(!res) throw std::runtime_error("Call to grid() failed."); - - Py_DECREF(args); - Py_DECREF(res); -} - -inline void show(const bool block = true) -{ - detail::_interpreter::get(); - - PyObject* res; - if(block) - { - res = PyObject_CallObject( - detail::_interpreter::get().s_python_function_show, - detail::_interpreter::get().s_python_empty_tuple); - } - else - { - PyObject *kwargs = PyDict_New(); - PyDict_SetItemString(kwargs, "block", Py_False); - res = PyObject_Call( detail::_interpreter::get().s_python_function_show, detail::_interpreter::get().s_python_empty_tuple, kwargs); - Py_DECREF(kwargs); - } - - - if (!res) throw std::runtime_error("Call to show() failed."); - - Py_DECREF(res); -} - -inline void close() -{ - detail::_interpreter::get(); - - PyObject* res = PyObject_CallObject( - detail::_interpreter::get().s_python_function_close, - detail::_interpreter::get().s_python_empty_tuple); - - if (!res) throw std::runtime_error("Call to close() failed."); - - Py_DECREF(res); -} - -inline void xkcd() { - detail::_interpreter::get(); - - PyObject* res; - PyObject *kwargs = PyDict_New(); - - res = PyObject_Call(detail::_interpreter::get().s_python_function_xkcd, - detail::_interpreter::get().s_python_empty_tuple, kwargs); - - Py_DECREF(kwargs); - - if (!res) - throw std::runtime_error("Call to show() failed."); - - Py_DECREF(res); -} - -inline void draw() -{ - detail::_interpreter::get(); - - PyObject* res = PyObject_CallObject( - detail::_interpreter::get().s_python_function_draw, - detail::_interpreter::get().s_python_empty_tuple); - - if (!res) throw std::runtime_error("Call to draw() failed."); - - Py_DECREF(res); -} - -template -inline void pause(Numeric interval) -{ - detail::_interpreter::get(); - - PyObject* args = PyTuple_New(1); - PyTuple_SetItem(args, 0, PyFloat_FromDouble(interval)); - - PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_pause, args); - if(!res) throw std::runtime_error("Call to pause() failed."); - - Py_DECREF(args); - Py_DECREF(res); -} - -inline void save(const std::string& filename) -{ - detail::_interpreter::get(); - - PyObject* pyfilename = PyString_FromString(filename.c_str()); - - PyObject* args = PyTuple_New(1); - PyTuple_SetItem(args, 0, pyfilename); - - PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_save, args); - if (!res) throw std::runtime_error("Call to save() failed."); - - Py_DECREF(args); - Py_DECREF(res); -} - -inline void clf() { - detail::_interpreter::get(); - - PyObject *res = PyObject_CallObject( - detail::_interpreter::get().s_python_function_clf, - detail::_interpreter::get().s_python_empty_tuple); - - if (!res) throw std::runtime_error("Call to clf() failed."); - - Py_DECREF(res); -} - -inline void cla() { - detail::_interpreter::get(); - - PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_cla, - detail::_interpreter::get().s_python_empty_tuple); - - if (!res) - throw std::runtime_error("Call to cla() failed."); - - Py_DECREF(res); -} - -inline void ion() { - detail::_interpreter::get(); - - PyObject *res = PyObject_CallObject( - detail::_interpreter::get().s_python_function_ion, - detail::_interpreter::get().s_python_empty_tuple); - - if (!res) throw std::runtime_error("Call to ion() failed."); - - Py_DECREF(res); -} - -inline std::vector> ginput(const int numClicks = 1, const std::map& keywords = {}) -{ - detail::_interpreter::get(); - - PyObject *args = PyTuple_New(1); - PyTuple_SetItem(args, 0, PyLong_FromLong(numClicks)); - - // construct keyword args - PyObject* kwargs = PyDict_New(); - for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) - { - PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); - } - - PyObject* res = PyObject_Call( - detail::_interpreter::get().s_python_function_ginput, args, kwargs); - - Py_DECREF(kwargs); - Py_DECREF(args); - if (!res) throw std::runtime_error("Call to ginput() failed."); - - const size_t len = PyList_Size(res); - std::vector> out; - out.reserve(len); - for (size_t i = 0; i < len; i++) { - PyObject *current = PyList_GetItem(res, i); - std::array position; - position[0] = PyFloat_AsDouble(PyTuple_GetItem(current, 0)); - position[1] = PyFloat_AsDouble(PyTuple_GetItem(current, 1)); - out.push_back(position); - } - Py_DECREF(res); - - return out; -} - -// Actually, is there any reason not to call this automatically for every plot? -inline void tight_layout() { - detail::_interpreter::get(); - - PyObject *res = PyObject_CallObject( - detail::_interpreter::get().s_python_function_tight_layout, - detail::_interpreter::get().s_python_empty_tuple); - - if (!res) throw std::runtime_error("Call to tight_layout() failed."); - - Py_DECREF(res); -} - -// Support for variadic plot() and initializer lists: - -namespace detail { - -template -using is_function = typename std::is_function>>::type; - -template -struct is_callable_impl; - -template -struct is_callable_impl -{ - typedef is_function type; -}; // a non-object is callable iff it is a function - -template -struct is_callable_impl -{ - struct Fallback { void operator()(); }; - struct Derived : T, Fallback { }; - - template struct Check; - - template - static std::true_type test( ... ); // use a variadic function to make sure (1) it accepts everything and (2) its always the worst match - - template - static std::false_type test( Check* ); - -public: - typedef decltype(test(nullptr)) type; - typedef decltype(&Fallback::operator()) dtype; - static constexpr bool value = type::value; -}; // an object is callable iff it defines operator() - -template -struct is_callable -{ - // dispatch to is_callable_impl or is_callable_impl depending on whether T is of class type or not - typedef typename is_callable_impl::value, T>::type type; -}; - -template -struct plot_impl { }; - -template<> -struct plot_impl -{ - template - bool operator()(const IterableX& x, const IterableY& y, const std::string& format) - { - detail::_interpreter::get(); - - // 2-phase lookup for distance, begin, end - using std::distance; - using std::begin; - using std::end; - - auto xs = distance(begin(x), end(x)); - auto ys = distance(begin(y), end(y)); - assert(xs == ys && "x and y data must have the same number of elements!"); - - PyObject* xlist = PyList_New(xs); - PyObject* ylist = PyList_New(ys); - PyObject* pystring = PyString_FromString(format.c_str()); - - auto itx = begin(x), ity = begin(y); - for(size_t i = 0; i < xs; ++i) { - PyList_SetItem(xlist, i, PyFloat_FromDouble(*itx++)); - PyList_SetItem(ylist, i, PyFloat_FromDouble(*ity++)); - } - - PyObject* plot_args = PyTuple_New(3); - PyTuple_SetItem(plot_args, 0, xlist); - PyTuple_SetItem(plot_args, 1, ylist); - PyTuple_SetItem(plot_args, 2, pystring); - - PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_plot, plot_args); - - Py_DECREF(plot_args); - if(res) Py_DECREF(res); - - return res; - } -}; - -template<> -struct plot_impl -{ - template - bool operator()(const Iterable& ticks, const Callable& f, const std::string& format) - { - if(begin(ticks) == end(ticks)) return true; - - // We could use additional meta-programming to deduce the correct element type of y, - // but all values have to be convertible to double anyways - std::vector y; - for(auto x : ticks) y.push_back(f(x)); - return plot_impl()(ticks,y,format); - } -}; - -} // end namespace detail - -// recursion stop for the above -template -bool plot() { return true; } - -template -bool plot(const A& a, const B& b, const std::string& format, Args... args) -{ - return detail::plot_impl::type>()(a,b,format) && plot(args...); -} - -/* - * This group of plot() functions is needed to support initializer lists, i.e. calling - * plot( {1,2,3,4} ) - */ -inline bool plot(const std::vector& x, const std::vector& y, const std::string& format = "") { - return plot(x,y,format); -} - -inline bool plot(const std::vector& y, const std::string& format = "") { - return plot(y,format); -} - -inline bool plot(const std::vector& x, const std::vector& y, const std::map& keywords) { - return plot(x,y,keywords); -} - -/* - * This class allows dynamic plots, ie changing the plotted data without clearing and re-plotting - */ -class Plot -{ -public: - // default initialization with plot label, some data and format - template - Plot(const std::string& name, const std::vector& x, const std::vector& y, const std::string& format = "") { - detail::_interpreter::get(); - - assert(x.size() == y.size()); - - PyObject* kwargs = PyDict_New(); - if(name != "") - PyDict_SetItemString(kwargs, "label", PyString_FromString(name.c_str())); - - PyObject* xarray = detail::get_array(x); - PyObject* yarray = detail::get_array(y); - - PyObject* pystring = PyString_FromString(format.c_str()); - - PyObject* plot_args = PyTuple_New(3); - PyTuple_SetItem(plot_args, 0, xarray); - PyTuple_SetItem(plot_args, 1, yarray); - PyTuple_SetItem(plot_args, 2, pystring); - - PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_plot, plot_args, kwargs); - - Py_DECREF(kwargs); - Py_DECREF(plot_args); - - if(res) - { - line= PyList_GetItem(res, 0); - - if(line) - set_data_fct = PyObject_GetAttrString(line,"set_data"); - else - Py_DECREF(line); - Py_DECREF(res); - } - } - - // shorter initialization with name or format only - // basically calls line, = plot([], []) - Plot(const std::string& name = "", const std::string& format = "") - : Plot(name, std::vector(), std::vector(), format) {} - - template - bool update(const std::vector& x, const std::vector& y) { - assert(x.size() == y.size()); - if(set_data_fct) - { - PyObject* xarray = detail::get_array(x); - PyObject* yarray = detail::get_array(y); - - PyObject* plot_args = PyTuple_New(2); - PyTuple_SetItem(plot_args, 0, xarray); - PyTuple_SetItem(plot_args, 1, yarray); - - PyObject* res = PyObject_CallObject(set_data_fct, plot_args); - if (res) Py_DECREF(res); - return res; - } - return false; - } - - // clears the plot but keep it available - bool clear() { - return update(std::vector(), std::vector()); - } - - // definitely remove this line - void remove() { - if(line) - { - auto remove_fct = PyObject_GetAttrString(line,"remove"); - PyObject* args = PyTuple_New(0); - PyObject* res = PyObject_CallObject(remove_fct, args); - if (res) Py_DECREF(res); - } - decref(); - } - - ~Plot() { - decref(); - } -private: - - void decref() { - if(line) - Py_DECREF(line); - if(set_data_fct) - Py_DECREF(set_data_fct); - } - - - PyObject* line = nullptr; - PyObject* set_data_fct = nullptr; -}; - -} // end namespace matplotlibcpp diff --git a/include/common/utilities/typeTrans.h b/include/common/utilities/typeTrans.h index 1dbb7ea..0f422e8 100755 --- a/include/common/utilities/typeTrans.h +++ b/include/common/utilities/typeTrans.h @@ -5,21 +5,6 @@ #include #include -/* Tools */ -// const std::string convert(const long l, const size_t n){ -// long val=l; -// std::stringstream ss; -// do -// { -// ss<<(val%n); -// val/=n; -// }while(val!=0); -// std::string s=ss.str(); -// std::reverse(s.begin(),s.end()); -// return s; -// } - - namespace typeTrans{ inline void addValue(std::vector &vec, double value){ diff --git a/include/control/CtrlComponents.h b/include/control/CtrlComponents.h index 5ee7b24..68c2c0b 100755 --- a/include/control/CtrlComponents.h +++ b/include/control/CtrlComponents.h @@ -1,37 +1,20 @@ #ifndef CTRLCOMPONENTS_H #define CTRLCOMPONENTS_H - #include "message/LowlevelCmd.h" #include "message/LowlevelState.h" #include "interface/IOInterface.h" #include "interface/IOROS.h" #include "model/ArmDynKineModel.h" -#include "model/ArmReal.h" #include "common/utilities/CSVTool.h" #include #include - -#ifdef COMPILE_DEBUG -#include "common/utilities/PyPlot.h" -#endif // COMPILE_DEBUG +#include "common/math/robotics.h" struct CtrlComponents{ public: - CtrlComponents(){} - ~CtrlComponents(){ - delete lowCmd; - delete lowState; - delete ioInter; - delete armModel; - delete stateCSV; - - - -#ifdef COMPILE_DEBUG - delete plot; -#endif // COMPILE_DEBUG - } + CtrlComponents(); + ~CtrlComponents(); int dof; std::string armConfigPath; @@ -40,45 +23,25 @@ public: IOInterface *ioInter; ArmDynKineModel *armModel; CSVTool *stateCSV; - -#ifdef COMPILE_DEBUG - PyPlot *plot; -#endif // COMPILE_DEBUG + + //config + Control ctrl; + bool _hasGripper; + std::string ctrl_IP; double dt; bool *running; - void sendRecv(){ - ioInter->sendRecv(lowCmd, lowState); - } - - void geneObj(){ - lowCmd = new LowlevelCmd(); - lowState = new LowlevelState(dt); - -#ifdef NO_GRIPPER - // armModel = new Z1DynKineModel(); - armModel = new Z1DynKineModel(Vec3(0.0, 0.0, 0.0), - 0.0, Vec3(0.0, 0.0, 0.0), Vec3(0.0, 0.0, 0.0).asDiagonal()); -#endif -#ifdef AG95 - armModel = new Z1DynKineModel(Vec3(0.21, 0, 0), - 1.107, Vec3(0.092, 0, 0), Vec3(0.005, 0.005, 0.005).asDiagonal()); -#endif -#ifdef MASS3KG - armModel = new Z1DynKineModel(Vec3(0.05, 0, 0), - 3.1, Vec3(0.05, 0, 0), Vec3(0.005, 0.005, 0.005).asDiagonal()); -#endif -#ifdef UNITREE_GRIPPER - // 安装面高出6mm - // armModel = new Z1DynKineModel(Vec3(0.141, 0, -0.005), - // 0.712, Vec3(0.0488, 0.0, 0.002), Vec3(0.000688, 0.000993, 0.000812).asDiagonal()); - armModel = new Z1DynKineModel(Vec3(0.0, 0.0, 0.0), - 0.80225, Vec3(0.0037, 0.0014, -0.0003), Vec3(0.00057593, 0.00099960, 0.00106337).asDiagonal()); -#endif - } + void sendRecv(); + void geneObj(); + bool isRecord; + void writeData(); private: + Vec3 _endPosLocal; + double _endEffectorMass; + Vec3 _endEffectorCom; + Mat3 _endEffectorInertia; }; #endif // CTRLCOMPONENTS_H \ No newline at end of file diff --git a/include/interface/IOInterface.h b/include/interface/IOInterface.h index a5ab94c..3829e82 100755 --- a/include/interface/IOInterface.h +++ b/include/interface/IOInterface.h @@ -4,9 +4,9 @@ #include "message/LowlevelCmd.h" #include "message/LowlevelState.h" #include "unitree_arm_sdk/cmdPanel.h" -#include "model/ArmReal.h" #include "unitree_arm_sdk/keyboard.h" #include +#include "common/math/robotics.h" class IOInterface{ public: @@ -19,17 +19,16 @@ public: std::string getString(std::string slogan){return _cmdPanel->getString(slogan);} std::vector stringToArray(std::string slogan){return _cmdPanel->stringToArray(slogan);} std::vector > stringToMatrix(std::string slogan){return _cmdPanel->stringToMatrix(slogan);} - virtual bool calibration(){}; + virtual bool calibration(){return false;}; -#ifdef CTRL_BY_SDK SendCmd getSendCmd(){return _cmdPanel->getSendCmd();}; void getRecvState(RecvState recvState){ _cmdPanel->getRecvState(recvState);}; -#endif + bool isDisConnect = false;// udp disconnection + bool _isDisConnect[7];// joint(motor) disconnection protected: + uint16_t _isDisConnectCnt[7]; CmdPanel *_cmdPanel; - - }; #endif //IOINTERFACE_H \ No newline at end of file diff --git a/include/interface/IOROS.h b/include/interface/IOROS.h index 7cdfeb8..4683553 100755 --- a/include/interface/IOROS.h +++ b/include/interface/IOROS.h @@ -11,25 +11,23 @@ class IOROS : public IOInterface{ public: - IOROS(CmdPanel *cmdPanel); + IOROS(CmdPanel *cmdPanel, bool hasGripper); ~IOROS(); bool sendRecv(const LowlevelCmd *cmd, LowlevelState *state); - // void setGripper(double position, double force){}; - // void getGripper(double &position, double &force){}; private: + bool _hasGripper; ros::NodeHandle _nm; -#ifndef UNITREE_GRIPPER - ros::Subscriber _servo_sub[6]; - ros::Publisher _servo_pub[6]; - unitree_legged_msgs::MotorState _joint_state[6]; - unitree_legged_msgs::MotorCmd _joint_cmd[6]; -#endif -#ifdef UNITREE_GRIPPER +// #ifdef UNITREE_GRIPPER ros::Subscriber _servo_sub[7]; ros::Publisher _servo_pub[7]; unitree_legged_msgs::MotorState _joint_state[7]; unitree_legged_msgs::MotorCmd _joint_cmd[7]; -#endif +// #else +// ros::Subscriber _servo_sub[6]; +// ros::Publisher _servo_pub[6]; +// unitree_legged_msgs::MotorState _joint_state[6]; +// unitree_legged_msgs::MotorCmd _joint_cmd[6]; +// #endif void _sendCmd(const LowlevelCmd *cmd); void _recvState(LowlevelState *state); void _initRecv(); @@ -42,7 +40,6 @@ private: void _joint04Callback(const unitree_legged_msgs::MotorState& msg); void _joint05Callback(const unitree_legged_msgs::MotorState& msg); void _gripperCallback(const unitree_legged_msgs::MotorState& msg); - }; #endif // IOROS_H diff --git a/include/interface/IOUDP.h b/include/interface/IOUDP.h index e3863d8..6ba770e 100644 --- a/include/interface/IOUDP.h +++ b/include/interface/IOUDP.h @@ -4,38 +4,9 @@ #include "interface/IOInterface.h" #include "/usr/include/x86_64-linux-gnu/bits/floatn-common.h" -// #pragma pack(1) -// struct JointCmd{ -// _Float32 T; -// _Float32 W; -// _Float32 Pos; -// _Float32 K_P; -// _Float32 K_W; -// }; - -// struct JointState{ -// _Float32 T; -// _Float32 W; -// _Float32 Acc; -// _Float32 Pos; -// }; - -// union SendCmd{ -// uint8_t checkCmd; -// JointCmd jointCmd[7]; -// }; - -// union RecvState{ -// uint8_t singleState; -// uint8_t selfCheck[10]; -// JointState jointState[7]; -// uint8_t errorCheck[16]; -// }; -// #pragma pack() - class IOUDP : public IOInterface{ public: - IOUDP(CmdPanel *cmdPanel); + IOUDP(CmdPanel *cmdPanel, const char* IP, bool hasGripper); ~IOUDP(); bool sendRecv(const LowlevelCmd *cmd, LowlevelState *state); // void setGripper(int position, int force); @@ -50,6 +21,9 @@ private: size_t _motorNum; size_t _jointNum; + + uint8_t _singleState; + uint8_t _selfCheck[10]; }; #endif // IOUDP_H \ No newline at end of file diff --git a/include/message/LowlevelCmd.h b/include/message/LowlevelCmd.h index aa07686..ed61262 100755 --- a/include/message/LowlevelCmd.h +++ b/include/message/LowlevelCmd.h @@ -16,24 +16,11 @@ public: std::vector kp; std::vector kd; + std::vector> q_cmd_data; + std::vector> dq_cmd_data; + std::vector> tau_cmd_data; - LowlevelCmd(){ -#ifndef UNITREE_GRIPPER - q.resize(_dof); - dq.resize(_dof); - tau.resize(_dof); - kp.resize(_dof); - kd.resize(_dof); -#endif - -#ifdef UNITREE_GRIPPER - q.resize(_dof+1); - dq.resize(_dof+1); - tau.resize(_dof+1); - kp.resize(_dof+1); - kd.resize(_dof+1); -#endif - } + LowlevelCmd(); ~LowlevelCmd(){} void setZeroDq(); @@ -51,10 +38,15 @@ public: void setGripperGain(float KP, float KW); void setGripperZeroGain(); void setGripperQ(double qInput); + double getGripperQ(); void setGripperQd(double qdInput); + double getGripperQd(); void setGripperTau(double tauInput); + double getGripperTau(); Vec6 getQ(); Vec6 getQd(); + + void resizeGripper(); }; diff --git a/include/message/LowlevelState.h b/include/message/LowlevelState.h index 6df090a..2117cc8 100755 --- a/include/message/LowlevelState.h +++ b/include/message/LowlevelState.h @@ -9,59 +9,12 @@ #include "common/utilities/LPFilter.h" #include "message/UserValue.h" -// struct MotorState -// { -// unsigned int mode; -// float q; -// float dq; -// float ddq; -// float tauEst; - -// MotorState(){ -// q = 0; -// dq = 0; -// ddq = 0; -// tauEst = 0; -// } -// }; - struct LowlevelState{ private: size_t _dof = 6; public: - LowlevelState(double dt){ -#ifndef UNITREE_GRIPPER - qFilter = new LPFilter(dt, 3.0, _dof); - dqFilter = new LPFilter(dt, 3.0, _dof); - tauFilter = new LPFilter(dt, 5.0, _dof); - - q.resize(_dof); - dq.resize(_dof); - ddq.resize(_dof); - tau.resize(_dof); - qFiltered.resize(_dof); - dqFiltered.resize(_dof); - tauFiltered.resize(_dof); -#endif -#ifdef UNITREE_GRIPPER - qFilter = new LPFilter(dt, 3.0, _dof+1); - dqFilter = new LPFilter(dt, 3.0, _dof+1); - tauFilter = new LPFilter(dt, 5.0, _dof+1); - - q.resize(_dof+1); - dq.resize(_dof+1); - ddq.resize(_dof+1); - tau.resize(_dof+1); - qFiltered.resize(_dof+1); - dqFiltered.resize(_dof+1); - tauFiltered.resize(_dof+1); -#endif - } - ~LowlevelState(){ - delete qFilter; - delete dqFilter; - delete tauFilter; - } + LowlevelState(double dt); + ~LowlevelState(); // MotorState motorState[12]; std::vector q; @@ -69,99 +22,36 @@ public: std::vector ddq; std::vector tau; + std::vector temperature; + std::vector errorstate; + std::vector qFiltered; std::vector dqFiltered; std::vector tauFiltered; + std::vector> q_state_data; + std::vector> dq_state_data; + std::vector> tau_state_data; + // ArmFSMStateName userCmd; UserValue userValue; LPFilter *qFilter; LPFilter *dqFilter; LPFilter *tauFilter; - void runFilter(){ - qFiltered = q; - qFilter->addValue(qFiltered); - - dqFiltered = dq; - dqFilter->addValue(dqFiltered); - - tauFiltered = tau; - tauFilter->addValue(tauFiltered); - } - - Vec6 getQ(){ - Vec6 qReturn; - for(int i(0); i < 6; ++i){ - qReturn(i) = q.at(i); - } - return qReturn; - } - - Vec6 getQd(){ - Vec6 qdReturn; - for(int i(0); i < 6; ++i){ - qdReturn(i) = dq.at(i); - } - return qdReturn; - } - - Vec6 getQdd(){ - Vec6 qddReturn; - for(int i(0); i < 6; ++i){ - qddReturn(i) = ddq.at(i); - } - return qddReturn; - } - - Vec6 getTau(){ - Vec6 tauReturn; - for(int i(0); i < 6; ++i){ - tauReturn(i) = tau.at(i); - } - return tauReturn; - } - - Vec6 getQFiltered(){ - Vec6 qReturn; - for(int i(0); i < 6; ++i){ - qReturn(i) = qFiltered.at(i); - } - return qReturn; - } - - Vec6 getQdFiltered(){ - Vec6 dqReturn; - for(int i(0); i < 6; ++i){ - dqReturn(i) = dqFiltered.at(i); - } - return dqReturn; - } - - - Vec6 getTauFiltered(){ - Vec6 tauReturn; - for(int i(0); i < 6; ++i){ - tauReturn(i) = tauFiltered.at(i); - } - return tauReturn; - } - - double getGripperQ(){ - return q.at(q.size()-1); - } - - double getGripperQd(){ - return dq.at(q.size()-1); - } - - double getGripperTau(){ - return tau.at(tau.size()-1); - } - - double getGripperTauFiltered(){ - return tauFiltered.at(tau.size()-1); - } + void resizeGripper(double dt); + void runFilter(); + Vec6 getQ(); + Vec6 getQd(); + Vec6 getQdd(); + Vec6 getTau(); + Vec6 getQFiltered(); + Vec6 getQdFiltered(); + Vec6 getTauFiltered(); + double getGripperQ() {return q.at(q.size()-1);} + double getGripperQd() {return dq.at(q.size()-1);} + double getGripperTau() {return tau.at(tau.size()-1);} + double getGripperTauFiltered() {return tauFiltered.at(tau.size()-1);} }; #endif //LOWLEVELSTATE_HPP diff --git a/include/message/UserValue.h b/include/message/UserValue.h index 1c208a1..e69956a 100644 --- a/include/message/UserValue.h +++ b/include/message/UserValue.h @@ -17,30 +17,12 @@ struct UserValue{ double gripperPos; void setData(std::vector rawData){ -#ifdef CTRL_BY_SDK if(rawData.size() != 7){ std::cout << "[ERROR] UserValue::setData, the size of rawDate is " << rawData.size() << " but not 7" << std::endl; } gripperPos = rawData.at(6); rawData = cutVector(rawData, 0, 6); moveAxis = typeTrans::getValue(rawData, moveAxis); -#endif -#ifdef CTRL_BY_KEYBOARD - if(rawData.size() != 7){ - std::cout << "[ERROR] UserValue::setData, the size of rawDate is " << rawData.size() << " but not 7" << std::endl; - } - gripperPos = rawData.at(6); - rawData = cutVector(rawData, 0, 6); - moveAxis = typeTrans::getValue(rawData, moveAxis); -#endif -#ifdef CTRL_BY_JOYSTICK - if(rawData.size() != 7){ - std::cout << "[ERROR] UserValue::setData, the size of rawDate is " << rawData.size() << " but not 7" << std::endl; - } - gripperPos = rawData.at(6); - rawData = cutVector(rawData, 0, 6); - moveAxis = typeTrans::getValue(rawData, moveAxis); -#endif } UserValue(){ diff --git a/include/model/ArmDynKineModel.h b/include/model/ArmDynKineModel.h index cb692e2..c82ad00 100755 --- a/include/model/ArmDynKineModel.h +++ b/include/model/ArmDynKineModel.h @@ -1,3 +1,4 @@ + #ifndef ARMDYNCKINEMODEL_H #define ARMDYNCKINEMODEL_H @@ -25,7 +26,7 @@ public: void solveQP(Vec6 twist, Vec6 pastAngle, Vec6 &result, double dt, bool setJ0QdZero, bool setQdZero); bool invKinematics(HomoMat gDes, Vec6 pastAngle, Vec6 &result, bool checkInWorkSpace = false); - HomoMat forwardKinematics(Vec6 q, int index=6); // index from 0 to 5, used for fourth version + HomoMat forwardKinematics(Vec6 q, int index=6, bool isExp = false); // index from 0 to 5, used for fourth version bool _useIKSolver = false; protected: @@ -111,4 +112,13 @@ public: }; -#endif // ARMDYNCKINEMODEL_H \ No newline at end of file +class Z1PlusDynKineModel : public ArmDynKineModel{ +public: + Z1PlusDynKineModel(Vec3 endPosLocal = Vec3::Zero(), + double endEffectorMass = 0.0, + Vec3 endEffectorCom = Vec3::Zero(), + Mat3 endEffectorInertia = Mat3::Zero()); + ~Z1PlusDynKineModel(){} +}; + +#endif \ No newline at end of file diff --git a/include/trajectory/EndCircleTraj.h b/include/trajectory/EndCircleTraj.h index 9306b99..f4e8701 100755 --- a/include/trajectory/EndCircleTraj.h +++ b/include/trajectory/EndCircleTraj.h @@ -16,10 +16,16 @@ public: void setEndRoundTraj(std::string stateName, Vec3 axisPointFromInit, Vec3 axisDirection, double maxSpeed, double angle, bool keepOrientation = true); + void setEndRoundTraj(Vec6 startP, Vec6 middleP, Vec6 endP, + double maxSpeed); private: + void _centerCircle(Vec3 p1, Vec3 p2, Vec3 p3); bool _getEndTraj(HomoMat &homo, Vec6 &twist); Vec3 _center; double _radius; + Vec6 _middlePosture; + Vec6 _middleQ; + HomoMat _middleHomo; HomoMat _centerHomo; HomoMat _initHomoToCenter; RotMat _initOri; diff --git a/include/trajectory/EndHomoTraj.h b/include/trajectory/EndHomoTraj.h index 138324a..6f29bd9 100755 --- a/include/trajectory/EndHomoTraj.h +++ b/include/trajectory/EndHomoTraj.h @@ -18,11 +18,17 @@ protected: HomoMat _cmdHomo; Vec6 _cmdTwist; - Vec6 _qPast; - // double _currentTime; + Vec6 _deltaPosture; + Vec3 _omgtheta; + double _theta; + Mat3 _startR, _endR, _delatR, _currentR; + Vec3 _startp, _endp, _currentp; SCurve *_sCurve; - +private: + bool checkInSingularity(); + void _checkAngleValid(const Vec6 &q, int pointOrder); + bool _checkJointAngleValid(const double &q, int jointOrder); }; -#endif // ENDHOMOTRAJ_H +#endif // ENDHOMOTRAJ_H \ No newline at end of file diff --git a/include/trajectory/EndLineTraj.h b/include/trajectory/EndLineTraj.h index 4fed4da..0c8fc75 100755 --- a/include/trajectory/EndLineTraj.h +++ b/include/trajectory/EndLineTraj.h @@ -12,7 +12,8 @@ public: void setEndLineTraj(HomoMat startHomo, Vec3 deltaPos, Vec3 deltaOri, double maxMovingSpeed, double maxTurningSpeed); void setEndLineTraj(std::string stateName, Vec3 deltaPos, Vec3 deltaOri, double maxMovingSpeed, double maxTurningSpeed); - + void setEndLineTraj(Vec6 startP, Vec6 endP, double maxMovingSpeed, double maxTurningSpeed); + void setEndLineTraj(std::string startName, std::string endName, double maxMovingSpeed, double maxTurningSpeed); private: bool _getEndTraj(HomoMat &homo, Vec6 &twist); @@ -22,9 +23,8 @@ private: Vec3 _currentDistance, _currentAngle; Vec3 _currentVelocity, _currentOmega; - SCurve _posCurve; SCurve _oriCurve; }; -#endif // ENDLINETRAJ_H \ No newline at end of file +#endif \ No newline at end of file diff --git a/include/trajectory/JointSpaceTraj.h b/include/trajectory/JointSpaceTraj.h index bbb2d4b..48c8d46 100755 --- a/include/trajectory/JointSpaceTraj.h +++ b/include/trajectory/JointSpaceTraj.h @@ -5,6 +5,7 @@ #include #include #include "control/CtrlComponents.h" +#include "trajectory/SCurve.h" class JointSpaceTraj : public Trajectory{ public: @@ -13,10 +14,11 @@ public: JointSpaceTraj(ArmDynKineModel *armModel, CSVTool *csvState); ~JointSpaceTraj(){} + void setGripper(double startQ, double endQ); bool getJointCmd(Vec6 &q, Vec6 &qd); bool getJointCmd(Vec6 &q, Vec6 &qd, double &gripperQ, double &gripperQd); void setJointTraj(Vec6 startQ, Vec6 endQ, double speed); - void setJointTraj(Vec6 startQ, std::string endName, double speed); + bool setJointTraj(Vec6 startQ, std::string endName, double speed); void setJointTraj(std::string startName, std::string endName, double speed); void setJointTraj(Vec6 startQ, Vec6 middleQ, double middleS, Vec6 endQ, double speed); @@ -33,10 +35,6 @@ private: double _pathTimeTemp; // path total time double _s, _sDot; // [0, 1] double _a3, _a4, _a5; // parameters of path - // double _speedFactor; // [0, 1] - std::vector _jointMaxQ; - std::vector _jointMinQ; - std::vector _jointMaxSpeed; int _trajOrder; // The order of trajectory curve std::vector _curveParam; diff --git a/include/trajectory/Trajectory.h b/include/trajectory/Trajectory.h index fa82fe2..108fab6 100755 --- a/include/trajectory/Trajectory.h +++ b/include/trajectory/Trajectory.h @@ -13,6 +13,9 @@ public: _csvState = ctrlComp->stateCSV; _dof = _armModel->getDOF(); _hasKinematic = true; + _jointMaxQ = _armModel->getJointQMax(); + _jointMinQ = _armModel->getJointQMin(); + _jointMaxSpeed = _armModel->getJointSpeedMax(); } Trajectory(ArmDynKineModel *armModel){ _ctrlComp = nullptr; @@ -22,26 +25,32 @@ public: if(_dof == 6){ _hasKinematic = true; } + _jointMaxQ = _armModel->getJointQMax(); + _jointMinQ = _armModel->getJointQMin(); + _jointMaxSpeed = _armModel->getJointSpeedMax(); } Trajectory(ArmDynKineModel *armModel, CSVTool *csvState):Trajectory(armModel){ _csvState = csvState; } virtual ~Trajectory(){} - virtual bool getJointCmd(Vec6 &q, Vec6 &qd){}; - virtual bool getJointCmd(Vec6 &q, Vec6 &qd, double &gripperQ, double &gripperQd){}; - virtual bool getCartesionCmd(Vec6 pastPosture, Vec6 &endPosture, Vec6 &endTwist){}; + virtual bool getJointCmd(Vec6 &q, Vec6 &qd){return false;}; + virtual bool getJointCmd(Vec6 &q, Vec6 &qd, double &gripperQ, double &gripperQd){return false;}; + virtual bool getCartesionCmd(Vec6 pastPosture, Vec6 &endPosture, Vec6 &endTwist){return false;}; void restart(){ _pathStarted = false; _reached = false; + _qPast = _startQ; } - void setGripper(double startQ, double endQ){ + virtual void setGripper(double startQ, double endQ){ _gripperStartQ = startQ; _gripperEndQ = endQ; } bool correctYN(){return _settingCorrect;} Vec6 getStartQ(){return _startQ;} Vec6 getEndQ(){return _endQ;} + double getEndGripperQ(){return _gripperEndQ;}; + double getStartGripperQ(){return _gripperStartQ;}; HomoMat getStartHomo(){ if(_hasKinematic){ return _startHomo; @@ -59,6 +68,14 @@ public: } } + Vec6 getEndPosture(){ + if(_hasKinematic){ + return _endPosture; + }else{ + std::cout << "[ERROR] This trajectory do not have kinematics" << std::endl; + exit(-1); + } + } double getPathTime(){return _pathTime;} protected: CtrlComponents *_ctrlComp; @@ -72,6 +89,9 @@ protected: double _currentTime; double _pathTime; double _tCost; + + Vec6 _qPast; + Vec6 _startPosture, _endPosture; Vec6 _startQ, _endQ; HomoMat _startHomo, _endHomo; @@ -81,6 +101,10 @@ protected: size_t _dof; bool _hasKinematic; + std::vector _jointMaxQ; + std::vector _jointMinQ; + std::vector _jointMaxSpeed; + void _runTime(){ _currentTime = getTimeSecond(); diff --git a/include/trajectory/TrajectoryManager.h b/include/trajectory/TrajectoryManager.h index 21af52a..9ba2e89 100755 --- a/include/trajectory/TrajectoryManager.h +++ b/include/trajectory/TrajectoryManager.h @@ -22,6 +22,9 @@ public: void emptyTraj(); Vec6 getStartQ(); Vec6 getEndQ(); + Vec6 getEndPosture(); + double getEndGripperQ(); + double getStartGripperQ(); HomoMat getEndHomo(); // bool checkTrajectoryContinuous(); private: diff --git a/lib/libZ1_JOYSTICK_ROS_Linux64.so b/lib/libZ1_JOYSTICK_ROS_Linux64.so deleted file mode 100755 index 1f32457..0000000 Binary files a/lib/libZ1_JOYSTICK_ROS_Linux64.so and /dev/null differ diff --git a/lib/libZ1_JOYSTICK_UDP_Linux64.so b/lib/libZ1_JOYSTICK_UDP_Linux64.so deleted file mode 100755 index fce861d..0000000 Binary files a/lib/libZ1_JOYSTICK_UDP_Linux64.so and /dev/null differ diff --git a/lib/libZ1_KEYBOARD_ROS_Linux64.so b/lib/libZ1_KEYBOARD_ROS_Linux64.so deleted file mode 100755 index d32f940..0000000 Binary files a/lib/libZ1_KEYBOARD_ROS_Linux64.so and /dev/null differ diff --git a/lib/libZ1_KEYBOARD_UDP_Linux64.so b/lib/libZ1_KEYBOARD_UDP_Linux64.so deleted file mode 100755 index 7c980e5..0000000 Binary files a/lib/libZ1_KEYBOARD_UDP_Linux64.so and /dev/null differ diff --git a/lib/libZ1_ROS_Linux64.so b/lib/libZ1_ROS_Linux64.so new file mode 100644 index 0000000..79bf905 Binary files /dev/null and b/lib/libZ1_ROS_Linux64.so differ diff --git a/lib/libZ1_SDK_ROS_Linux64.so b/lib/libZ1_SDK_ROS_Linux64.so deleted file mode 100755 index f3b7dc5..0000000 Binary files a/lib/libZ1_SDK_ROS_Linux64.so and /dev/null differ diff --git a/lib/libZ1_SDK_UDP_Linux64.so b/lib/libZ1_SDK_UDP_Linux64.so deleted file mode 100755 index fac7e1f..0000000 Binary files a/lib/libZ1_SDK_UDP_Linux64.so and /dev/null differ diff --git a/lib/libZ1_UDP_Linux64.so b/lib/libZ1_UDP_Linux64.so new file mode 100644 index 0000000..85f9b2a Binary files /dev/null and b/lib/libZ1_UDP_Linux64.so differ diff --git a/main.cpp b/main.cpp index 14df459..40e1720 100755 --- a/main.cpp +++ b/main.cpp @@ -4,7 +4,6 @@ #include #include #include "control/CtrlComponents.h" -#include "model/ArmReal.h" #include "model/ArmDynKineModel.h" #include "interface/IOUDP.h" @@ -24,6 +23,7 @@ #include "FSM/State_Calibration.h" #include "FSM/State_LowCmd.h" #include "FSM/FiniteStateMachine.h" +#include "FSM/State_SetTraj.h" #include "unitree_arm_sdk/unitree_arm_sdk.h" @@ -35,6 +35,7 @@ void ShutDown(int sig){ running = false; } +//set real-time program void setProcessScheduler(){ pid_t pid = getpid(); sched_param param; @@ -53,144 +54,93 @@ int main(int argc, char **argv){ EmptyAction emptyAction((int)ArmFSMStateName::INVALID); std::vector events; CmdPanel *cmdPanel; -#ifdef CTRL_BY_SDK - events.push_back(new StateAction("`", (int)ArmFSMStateName::BACKTOSTART)); - events.push_back(new StateAction("1", (int)ArmFSMStateName::PASSIVE)); - events.push_back(new StateAction("2", (int)ArmFSMStateName::JOINTCTRL)); - events.push_back(new StateAction("3", (int)ArmFSMStateName::CARTESIAN)); - events.push_back(new StateAction("4", (int)ArmFSMStateName::MOVEJ)); - events.push_back(new StateAction("5", (int)ArmFSMStateName::MOVEL)); - events.push_back(new StateAction("6", (int)ArmFSMStateName::MOVEC)); - events.push_back(new StateAction("7", (int)ArmFSMStateName::TEACH)); - events.push_back(new StateAction("8", (int)ArmFSMStateName::TEACHREPEAT)); - events.push_back(new StateAction("9", (int)ArmFSMStateName::SAVESTATE)); - events.push_back(new StateAction("0", (int)ArmFSMStateName::TOSTATE)); - events.push_back(new StateAction("-", (int)ArmFSMStateName::TRAJECTORY)); - events.push_back(new StateAction("=", (int)ArmFSMStateName::CALIBRATION)); - events.push_back(new StateAction("]", (int)ArmFSMStateName::NEXT)); - events.push_back(new StateAction("/", (int)ArmFSMStateName::LOWCMD)); + CtrlComponents *ctrlComp = new CtrlComponents(); - events.push_back(new ValueAction("q", "a", 1.0)); - events.push_back(new ValueAction("w", "s", 1.0)); - events.push_back(new ValueAction("e", "d", 1.0)); - events.push_back(new ValueAction("r", "f", 1.0)); - events.push_back(new ValueAction("t", "g", 1.0)); - events.push_back(new ValueAction("y", "h", 1.0)); - events.push_back(new ValueAction("down", "up", 1.0)); - - cmdPanel = new UnitreeKeyboardUDPRecv(events, emptyAction); -#endif -#ifdef CTRL_BY_KEYBOARD - events.push_back(new StateAction("`", (int)ArmFSMStateName::BACKTOSTART)); - events.push_back(new StateAction("1", (int)ArmFSMStateName::PASSIVE)); - events.push_back(new StateAction("2", (int)ArmFSMStateName::JOINTCTRL)); - events.push_back(new StateAction("3", (int)ArmFSMStateName::CARTESIAN)); - events.push_back(new StateAction("4", (int)ArmFSMStateName::MOVEJ)); - events.push_back(new StateAction("5", (int)ArmFSMStateName::MOVEL)); - events.push_back(new StateAction("6", (int)ArmFSMStateName::MOVEC)); - events.push_back(new StateAction("7", (int)ArmFSMStateName::TEACH)); - events.push_back(new StateAction("8", (int)ArmFSMStateName::TEACHREPEAT)); - events.push_back(new StateAction("9", (int)ArmFSMStateName::SAVESTATE)); - events.push_back(new StateAction("0", (int)ArmFSMStateName::TOSTATE)); - events.push_back(new StateAction("-", (int)ArmFSMStateName::TRAJECTORY)); - events.push_back(new StateAction("=", (int)ArmFSMStateName::CALIBRATION)); - events.push_back(new StateAction("]", (int)ArmFSMStateName::NEXT)); + if(ctrlComp->ctrl == Control::_SDK) + { + events.push_back(new StateAction("`", (int)ArmFSMStateName::BACKTOSTART)); + events.push_back(new StateAction("1", (int)ArmFSMStateName::PASSIVE)); + events.push_back(new StateAction("2", (int)ArmFSMStateName::JOINTCTRL)); + events.push_back(new StateAction("3", (int)ArmFSMStateName::CARTESIAN)); + events.push_back(new StateAction("4", (int)ArmFSMStateName::MOVEJ)); + events.push_back(new StateAction("5", (int)ArmFSMStateName::MOVEL)); + events.push_back(new StateAction("6", (int)ArmFSMStateName::MOVEC)); + events.push_back(new StateAction("7", (int)ArmFSMStateName::TEACH)); + events.push_back(new StateAction("8", (int)ArmFSMStateName::TEACHREPEAT)); + events.push_back(new StateAction("9", (int)ArmFSMStateName::SAVESTATE)); + events.push_back(new StateAction("0", (int)ArmFSMStateName::TOSTATE)); + events.push_back(new StateAction("-", (int)ArmFSMStateName::TRAJECTORY)); + events.push_back(new StateAction("=", (int)ArmFSMStateName::CALIBRATION)); + events.push_back(new StateAction("]", (int)ArmFSMStateName::NEXT)); + events.push_back(new StateAction("/", (int)ArmFSMStateName::LOWCMD)); + events.push_back(new StateAction("l", (int)ArmFSMStateName::SETTRAJ)); - events.push_back(new ValueAction("q", "a", 1.0)); - events.push_back(new ValueAction("w", "s", 1.0)); - events.push_back(new ValueAction("e", "d", 1.0)); - events.push_back(new ValueAction("r", "f", 1.0)); - events.push_back(new ValueAction("t", "g", 1.0)); - events.push_back(new ValueAction("y", "h", 1.0)); - events.push_back(new ValueAction("down", "up", 1.0)); + events.push_back(new ValueAction("q", "a", 1.0)); + events.push_back(new ValueAction("w", "s", 1.0)); + events.push_back(new ValueAction("e", "d", 1.0)); + events.push_back(new ValueAction("r", "f", 1.0)); + events.push_back(new ValueAction("t", "g", 1.0)); + events.push_back(new ValueAction("y", "h", 1.0)); + events.push_back(new ValueAction("down", "up", 1.0)); + + cmdPanel = new UnitreeKeyboardUDPRecv(events, emptyAction); + }else if(ctrlComp->ctrl == Control::_KEYBOARD){ + events.push_back(new StateAction("`", (int)ArmFSMStateName::BACKTOSTART)); + events.push_back(new StateAction("1", (int)ArmFSMStateName::PASSIVE)); + events.push_back(new StateAction("2", (int)ArmFSMStateName::JOINTCTRL)); + events.push_back(new StateAction("3", (int)ArmFSMStateName::CARTESIAN)); + events.push_back(new StateAction("4", (int)ArmFSMStateName::MOVEJ)); + events.push_back(new StateAction("5", (int)ArmFSMStateName::MOVEL)); + events.push_back(new StateAction("6", (int)ArmFSMStateName::MOVEC)); + events.push_back(new StateAction("7", (int)ArmFSMStateName::TEACH)); + events.push_back(new StateAction("8", (int)ArmFSMStateName::TEACHREPEAT)); + events.push_back(new StateAction("9", (int)ArmFSMStateName::SAVESTATE)); + events.push_back(new StateAction("0", (int)ArmFSMStateName::TOSTATE)); + events.push_back(new StateAction("-", (int)ArmFSMStateName::TRAJECTORY)); + events.push_back(new StateAction("=", (int)ArmFSMStateName::CALIBRATION)); + events.push_back(new StateAction("]", (int)ArmFSMStateName::NEXT)); - cmdPanel = new Keyboard(events, emptyAction); -#endif -#ifdef CTRL_BY_JOYSTICK - events.push_back(new StateAction("r12_passive", (int)ArmFSMStateName::PASSIVE)); - events.push_back(new StateAction("r2_backToState", (int)ArmFSMStateName::BACKTOSTART)); - events.push_back(new StateAction("r1_cartesian", (int)ArmFSMStateName::CARTESIAN)); - events.push_back(new StateAction("select", (int)ArmFSMStateName::NEXT)); + events.push_back(new ValueAction("q", "a", 1.0)); + events.push_back(new ValueAction("w", "s", 1.0)); + events.push_back(new ValueAction("e", "d", 1.0)); + events.push_back(new ValueAction("r", "f", 1.0)); + events.push_back(new ValueAction("t", "g", 1.0)); + events.push_back(new ValueAction("y", "h", 1.0)); + events.push_back(new ValueAction("down", "up", 1.0)); + + cmdPanel = new Keyboard(events, emptyAction); + }else if(ctrlComp->ctrl == Control::_JOYSTICK){ + events.push_back(new StateAction("r12", (int)ArmFSMStateName::PASSIVE)); + events.push_back(new StateAction("l12", (int)ArmFSMStateName::PASSIVE)); + events.push_back(new StateAction("r2", (int)ArmFSMStateName::JOINTCTRL)); + events.push_back(new StateAction("r1", (int)ArmFSMStateName::CARTESIAN)); + events.push_back(new StateAction("select", (int)ArmFSMStateName::BACKTOSTART)); - events.push_back(new ValueAction("ly_forTranY", "ly_invTranY", 1.0)); - events.push_back(new ValueAction("lx_forTranX", "lx_invTranX", 1.0)); - events.push_back(new ValueAction("up_forTranZ", "down_invTranZ", 1.0)); - events.push_back(new ValueAction("ry_forRotY", "ry_invRotY", 1.0)); - events.push_back(new ValueAction("rx_forRotX", "rx_invRotX", 1.0)); - events.push_back(new ValueAction("Y_forRotZ", "A_invRotZ", 1.0)); - events.push_back(new ValueAction("right_gClose", "left_gOpen", 1.0)); - // events.push_back(new ValueAction("X_velDown", "B_velUp", 1.0)); + events.push_back(new ValueAction("left_up", "left_down", 1.0));//Tran_Y + events.push_back(new ValueAction("left_left", "left_right", -1.0));//Tran_X, inverse + events.push_back(new ValueAction("up", "down", 1.0));//Tran_Z + events.push_back(new ValueAction("right_up", "right_down", 1.0));//Rot_Y + events.push_back(new ValueAction("right_left", "right_right", 1.0));//Rot_x + events.push_back(new ValueAction("Y", "A", 1.0));//Rot_Z + events.push_back(new ValueAction("right", "left", 1.0));//girpper, close-open - cmdPanel = new UnitreeJoystick(events, emptyAction); -#endif + cmdPanel = new UnitreeJoystick(events, emptyAction); + } #ifdef RUN_ROS ros::init(argc, argv, "z1_controller"); #endif // RUN_ROS - CtrlComponents *ctrlComp = new CtrlComponents(); ctrlComp->dof = 6; ctrlComp->armConfigPath = "../config/"; ctrlComp->stateCSV = new CSVTool("../config/savedArmStates.csv"); ctrlComp->running = &running; - #ifdef UDP - ctrlComp->ioInter = new IOUDP(cmdPanel); - ctrlComp->dt = 0.004; + ctrlComp->ioInter = new IOUDP(cmdPanel, ctrlComp->ctrl_IP.c_str(), ctrlComp->_hasGripper); +#elif defined ROS + ctrlComp->ioInter = new IOROS(cmdPanel, ctrlComp->_hasGripper); #endif -#ifdef ROS - ctrlComp->ioInter = new IOROS(cmdPanel); ctrlComp->dt = 0.004; -#endif - -#ifdef COMPILE_DEBUG -std::cout << "add plot" << std::endl; - - ctrlComp->plot = new PyPlot(); - /* for general debugging */ - // ctrlComp->plot->addPlot("qCmd", 6); - // ctrlComp->plot->addPlot("qdCmd", 6); - // ctrlComp->plot->addPlot("tauCmd", 6); - - // ctrlComp->plot->addPlot("qReal", 6); - // ctrlComp->plot->addPlot("qdReal", 6); - // ctrlComp->plot->addPlot("realTau", 6); - - // ctrlComp->plot->addPlot("errorTau", 6); - - // ctrlComp->plot->addPlot("error", 1); - // ctrlComp->plot->addPlot("costTime", 1); - - /* for MoveJ, MoveL, MoveC*/ - // ctrlComp->plot->addPlot("moveLPostureDes", 6); - // ctrlComp->plot->addPlot("moveLTwistDes", 6); - // ctrlComp->plot->addPlot("moveLPostureAct", 6); - // ctrlComp->plot->addPlot("moveLTwistAct", 6); - // ctrlComp->plot->addPlot("moveLPostureError", 6); - // ctrlComp->plot->addPlot("moveLTwistError", 6); - - // /* for cartesian */ - // ctrlComp->plot->addPlot("cartesianPostureDes", 6); - // ctrlComp->plot->addPlot("cartesianTwistDes", 6); - // ctrlComp->plot->addPlot("cartesianPostureAct", 6); - // ctrlComp->plot->addPlot("cartesianTwistAct", 6); - // ctrlComp->plot->addPlot("cartesianPostureError", 6); - // ctrlComp->plot->addPlot("cartesianTwistError", 6); - - /* for trajectory */ - // plot->addPlot("s", 1); - // plot->addPlot("qPath", 6); - // plot->addPlot("qdPath", 6); - // plot->addPlot("qdPathDiff", 6); - // ctrlComp->plot->addPlot("qState", 6); - // ctrlComp->plot->addPlot("qError", 6); - - /* for gripper */ - // ctrlComp->plot->addPlot("gripper q error", 1); - // ctrlComp->plot->addPlot("gripper qd error", 1); - // ctrlComp->plot->addPlot("gripper tau", 1); - -#endif // COMPILE_DEBUG ctrlComp->geneObj(); std::vector states; @@ -208,41 +158,20 @@ std::cout << "add plot" << std::endl; states.push_back(new State_ToState(ctrlComp)); states.push_back(new State_Trajectory(ctrlComp)); states.push_back(new State_Calibration(ctrlComp)); - - std::vector _trajMag; - std::vector _jointTraj; - - /* start to forward */ - _trajMag.push_back(new TrajectoryManager(ctrlComp)); - _jointTraj.push_back(new JointSpaceTraj(ctrlComp)); - _jointTraj.at(_jointTraj.size()-1)->setJointTraj("startFlat", "forward", 0.5); - _trajMag.at(_trajMag.size()-1)->addTrajectory(_jointTraj.at(_jointTraj.size()-1)); - - /* forward to start */ - _trajMag.push_back(new TrajectoryManager(ctrlComp)); - _jointTraj.push_back(new JointSpaceTraj(ctrlComp)); - _jointTraj.at(_jointTraj.size()-1)->setJointTraj("forward", "startFlat", 0.5); - _trajMag.at(_trajMag.size()-1)->addTrajectory(_jointTraj.at(_jointTraj.size()-1)); - - states.push_back(new State_Dance(ctrlComp, _trajMag.at(0), ArmFSMStateName::DANCE00, ArmFSMStateName::DANCE01, "dance 00")); - states.push_back(new State_Dance(ctrlComp, _trajMag.at(1), ArmFSMStateName::DANCE01, ArmFSMStateName::TRAJECTORY, "dance 01")); -// #endif + states.push_back(new State_SetTraj(ctrlComp)); FiniteStateMachine fsm(states, cmdPanel, 0, ctrlComp->dt); signal(SIGINT, ShutDown); - // std::vector values; while(running){ - // values = cmdPanel->getValues(); - // std::cout << "value: " << values.at(0) << std::endl; usleep(100000); } #ifdef COMPILE_DEBUG -std::cout << "show plot" << std::endl; - ctrlComp->plot->showPlotAll(); -#endif // COMPILE_DEBUG + ctrlComp->writeData(); +#endif + delete ctrlComp; return 0; diff --git a/thirdparty/eigen-3.3.9.zip b/thirdparty/eigen-3.3.9.zip deleted file mode 100644 index ecdd007..0000000 Binary files a/thirdparty/eigen-3.3.9.zip and /dev/null differ diff --git a/thirdparty/quadProgpp/include/Array.hh b/thirdparty/quadProgpp/include/Array.hh index 7d7244b..29e494d 100755 --- a/thirdparty/quadProgpp/include/Array.hh +++ b/thirdparty/quadProgpp/include/Array.hh @@ -1785,7 +1785,7 @@ void svd(const Matrix& A, Matrix& U, Vector& W, Matrix& V) for (unsigned int its = 0; its < max_its; its++) { flag = true; - for (l = k; l >= 0; l--) // FIXME: in NR it was l >= 1 but there subscripts start from one + for (l = k; l >= 0; l--) { // Test for splitting nm = l - 1; // Note that rV[0] is always zero if ((T)(fabs(rv1[l]) + anorm) == anorm) diff --git a/thirdparty/quadProgpp/src/QuadProg++.cc b/thirdparty/quadProgpp/src/QuadProg++.cc index dc78e5e..f135c32 100755 --- a/thirdparty/quadProgpp/src/QuadProg++.cc +++ b/thirdparty/quadProgpp/src/QuadProg++.cc @@ -338,7 +338,6 @@ l2a:/* Step 2a: determine step direction */ if (t >= inf) { /* QPP is infeasible */ - // FIXME: unbounded to raise return inf; } /* case (ii): step in dual space */ diff --git a/thirdparty/rbdl-2.6.0.zip b/thirdparty/rbdl-2.6.0.zip deleted file mode 100644 index 6208944..0000000 Binary files a/thirdparty/rbdl-2.6.0.zip and /dev/null differ diff --git a/thirdparty/tinyxml/include/tinystr.h b/thirdparty/tinyxml/include/tinystr.h new file mode 100644 index 0000000..89cca33 --- /dev/null +++ b/thirdparty/tinyxml/include/tinystr.h @@ -0,0 +1,305 @@ +/* +www.sourceforge.net/projects/tinyxml + +This software is provided 'as-is', without any express or implied +warranty. In no event will the authors be held liable for any +damages arising from the use of this software. + +Permission is granted to anyone to use this software for any +purpose, including commercial applications, and to alter it and +redistribute it freely, subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must +not claim that you wrote the original software. If you use this +software in a product, an acknowledgment in the product documentation +would be appreciated but is not required. + +2. Altered source versions must be plainly marked as such, and +must not be misrepresented as being the original software. + +3. This notice may not be removed or altered from any source +distribution. +*/ + + +#ifndef TIXML_USE_STL + +#ifndef TIXML_STRING_INCLUDED +#define TIXML_STRING_INCLUDED + +#include +#include + +/* The support for explicit isn't that universal, and it isn't really + required - it is used to check that the TiXmlString class isn't incorrectly + used. Be nice to old compilers and macro it here: +*/ +#if defined(_MSC_VER) && (_MSC_VER >= 1200 ) + // Microsoft visual studio, version 6 and higher. + #define TIXML_EXPLICIT explicit +#elif defined(__GNUC__) && (__GNUC__ >= 3 ) + // GCC version 3 and higher.s + #define TIXML_EXPLICIT explicit +#else + #define TIXML_EXPLICIT +#endif + + +/* + TiXmlString is an emulation of a subset of the std::string template. + Its purpose is to allow compiling TinyXML on compilers with no or poor STL support. + Only the member functions relevant to the TinyXML project have been implemented. + The buffer allocation is made by a simplistic power of 2 like mechanism : if we increase + a string and there's no more room, we allocate a buffer twice as big as we need. +*/ +class TiXmlString +{ + public : + // The size type used + typedef size_t size_type; + + // Error value for find primitive + static const size_type npos; // = -1; + + + // TiXmlString empty constructor + TiXmlString () : rep_(&nullrep_) + { + } + + // TiXmlString copy constructor + TiXmlString ( const TiXmlString & copy) : rep_(0) + { + init(copy.length()); + memcpy(start(), copy.data(), length()); + } + + // TiXmlString constructor, based on a string + TIXML_EXPLICIT TiXmlString ( const char * copy) : rep_(0) + { + init( static_cast( strlen(copy) )); + memcpy(start(), copy, length()); + } + + // TiXmlString constructor, based on a string + TIXML_EXPLICIT TiXmlString ( const char * str, size_type len) : rep_(0) + { + init(len); + memcpy(start(), str, len); + } + + // TiXmlString destructor + ~TiXmlString () + { + quit(); + } + + TiXmlString& operator = (const char * copy) + { + return assign( copy, (size_type)strlen(copy)); + } + + TiXmlString& operator = (const TiXmlString & copy) + { + return assign(copy.start(), copy.length()); + } + + + // += operator. Maps to append + TiXmlString& operator += (const char * suffix) + { + return append(suffix, static_cast( strlen(suffix) )); + } + + // += operator. Maps to append + TiXmlString& operator += (char single) + { + return append(&single, 1); + } + + // += operator. Maps to append + TiXmlString& operator += (const TiXmlString & suffix) + { + return append(suffix.data(), suffix.length()); + } + + + // Convert a TiXmlString into a null-terminated char * + const char * c_str () const { return rep_->str; } + + // Convert a TiXmlString into a char * (need not be null terminated). + const char * data () const { return rep_->str; } + + // Return the length of a TiXmlString + size_type length () const { return rep_->size; } + + // Alias for length() + size_type size () const { return rep_->size; } + + // Checks if a TiXmlString is empty + bool empty () const { return rep_->size == 0; } + + // Return capacity of string + size_type capacity () const { return rep_->capacity; } + + + // single char extraction + const char& at (size_type index) const + { + assert( index < length() ); + return rep_->str[ index ]; + } + + // [] operator + char& operator [] (size_type index) const + { + assert( index < length() ); + return rep_->str[ index ]; + } + + // find a char in a string. Return TiXmlString::npos if not found + size_type find (char lookup) const + { + return find(lookup, 0); + } + + // find a char in a string from an offset. Return TiXmlString::npos if not found + size_type find (char tofind, size_type offset) const + { + if (offset >= length()) return npos; + + for (const char* p = c_str() + offset; *p != '\0'; ++p) + { + if (*p == tofind) return static_cast< size_type >( p - c_str() ); + } + return npos; + } + + void clear () + { + //Lee: + //The original was just too strange, though correct: + // TiXmlString().swap(*this); + //Instead use the quit & re-init: + quit(); + init(0,0); + } + + /* Function to reserve a big amount of data when we know we'll need it. Be aware that this + function DOES NOT clear the content of the TiXmlString if any exists. + */ + void reserve (size_type cap); + + TiXmlString& assign (const char* str, size_type len); + + TiXmlString& append (const char* str, size_type len); + + void swap (TiXmlString& other) + { + Rep* r = rep_; + rep_ = other.rep_; + other.rep_ = r; + } + + private: + + void init(size_type sz) { init(sz, sz); } + void set_size(size_type sz) { rep_->str[ rep_->size = sz ] = '\0'; } + char* start() const { return rep_->str; } + char* finish() const { return rep_->str + rep_->size; } + + struct Rep + { + size_type size, capacity; + char str[1]; + }; + + void init(size_type sz, size_type cap) + { + if (cap) + { + // Lee: the original form: + // rep_ = static_cast(operator new(sizeof(Rep) + cap)); + // doesn't work in some cases of new being overloaded. Switching + // to the normal allocation, although use an 'int' for systems + // that are overly picky about structure alignment. + const size_type bytesNeeded = sizeof(Rep) + cap; + const size_type intsNeeded = ( bytesNeeded + sizeof(int) - 1 ) / sizeof( int ); + rep_ = reinterpret_cast( new int[ intsNeeded ] ); + + rep_->str[ rep_->size = sz ] = '\0'; + rep_->capacity = cap; + } + else + { + rep_ = &nullrep_; + } + } + + void quit() + { + if (rep_ != &nullrep_) + { + // The rep_ is really an array of ints. (see the allocator, above). + // Cast it back before delete, so the compiler won't incorrectly call destructors. + delete [] ( reinterpret_cast( rep_ ) ); + } + } + + Rep * rep_; + static Rep nullrep_; + +} ; + + +inline bool operator == (const TiXmlString & a, const TiXmlString & b) +{ + return ( a.length() == b.length() ) // optimization on some platforms + && ( strcmp(a.c_str(), b.c_str()) == 0 ); // actual compare +} +inline bool operator < (const TiXmlString & a, const TiXmlString & b) +{ + return strcmp(a.c_str(), b.c_str()) < 0; +} + +inline bool operator != (const TiXmlString & a, const TiXmlString & b) { return !(a == b); } +inline bool operator > (const TiXmlString & a, const TiXmlString & b) { return b < a; } +inline bool operator <= (const TiXmlString & a, const TiXmlString & b) { return !(b < a); } +inline bool operator >= (const TiXmlString & a, const TiXmlString & b) { return !(a < b); } + +inline bool operator == (const TiXmlString & a, const char* b) { return strcmp(a.c_str(), b) == 0; } +inline bool operator == (const char* a, const TiXmlString & b) { return b == a; } +inline bool operator != (const TiXmlString & a, const char* b) { return !(a == b); } +inline bool operator != (const char* a, const TiXmlString & b) { return !(b == a); } + +TiXmlString operator + (const TiXmlString & a, const TiXmlString & b); +TiXmlString operator + (const TiXmlString & a, const char* b); +TiXmlString operator + (const char* a, const TiXmlString & b); + + +/* + TiXmlOutStream is an emulation of std::ostream. It is based on TiXmlString. + Only the operators that we need for TinyXML have been developped. +*/ +class TiXmlOutStream : public TiXmlString +{ +public : + + // TiXmlOutStream << operator. + TiXmlOutStream & operator << (const TiXmlString & in) + { + *this += in; + return *this; + } + + // TiXmlOutStream << operator. + TiXmlOutStream & operator << (const char * in) + { + *this += in; + return *this; + } + +} ; + +#endif // TIXML_STRING_INCLUDED +#endif // TIXML_USE_STL diff --git a/thirdparty/tinyxml/include/tinyxml.h b/thirdparty/tinyxml/include/tinyxml.h new file mode 100644 index 0000000..a3589e5 --- /dev/null +++ b/thirdparty/tinyxml/include/tinyxml.h @@ -0,0 +1,1805 @@ +/* +www.sourceforge.net/projects/tinyxml +Original code by Lee Thomason (www.grinninglizard.com) + +This software is provided 'as-is', without any express or implied +warranty. In no event will the authors be held liable for any +damages arising from the use of this software. + +Permission is granted to anyone to use this software for any +purpose, including commercial applications, and to alter it and +redistribute it freely, subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must +not claim that you wrote the original software. If you use this +software in a product, an acknowledgment in the product documentation +would be appreciated but is not required. + +2. Altered source versions must be plainly marked as such, and +must not be misrepresented as being the original software. + +3. This notice may not be removed or altered from any source +distribution. +*/ + + +#ifndef TINYXML_INCLUDED +#define TINYXML_INCLUDED + +#ifdef _MSC_VER +#pragma warning( push ) +#pragma warning( disable : 4530 ) +#pragma warning( disable : 4786 ) +#endif + +#include +#include +#include +#include +#include + +// Help out windows: +#if defined( _DEBUG ) && !defined( DEBUG ) +#define DEBUG +#endif + +#ifdef TIXML_USE_STL + #include + #include + #include + #define TIXML_STRING std::string +#else + #include "tinystr.h" + #define TIXML_STRING TiXmlString +#endif + +// Deprecated library function hell. Compilers want to use the +// new safe versions. This probably doesn't fully address the problem, +// but it gets closer. There are too many compilers for me to fully +// test. If you get compilation troubles, undefine TIXML_SAFE +#define TIXML_SAFE + +#ifdef TIXML_SAFE + #if defined(_MSC_VER) && (_MSC_VER >= 1400 ) + // Microsoft visual studio, version 2005 and higher. + #define TIXML_SNPRINTF _snprintf_s + #define TIXML_SSCANF sscanf_s + #elif defined(_MSC_VER) && (_MSC_VER >= 1200 ) + // Microsoft visual studio, version 6 and higher. + //#pragma message( "Using _sn* functions." ) + #define TIXML_SNPRINTF _snprintf + #define TIXML_SSCANF sscanf + #elif defined(__GNUC__) && (__GNUC__ >= 3 ) + // GCC version 3 and higher.s + //#warning( "Using sn* functions." ) + #define TIXML_SNPRINTF snprintf + #define TIXML_SSCANF sscanf + #else + #define TIXML_SNPRINTF snprintf + #define TIXML_SSCANF sscanf + #endif +#endif + +class TiXmlDocument; +class TiXmlElement; +class TiXmlComment; +class TiXmlUnknown; +class TiXmlAttribute; +class TiXmlText; +class TiXmlDeclaration; +class TiXmlParsingData; + +const int TIXML_MAJOR_VERSION = 2; +const int TIXML_MINOR_VERSION = 6; +const int TIXML_PATCH_VERSION = 2; + +/* Internal structure for tracking location of items + in the XML file. +*/ +struct TiXmlCursor +{ + TiXmlCursor() { Clear(); } + void Clear() { row = col = -1; } + + int row; // 0 based. + int col; // 0 based. +}; + + +/** + Implements the interface to the "Visitor pattern" (see the Accept() method.) + If you call the Accept() method, it requires being passed a TiXmlVisitor + class to handle callbacks. For nodes that contain other nodes (Document, Element) + you will get called with a VisitEnter/VisitExit pair. Nodes that are always leaves + are simply called with Visit(). + + If you return 'true' from a Visit method, recursive parsing will continue. If you return + false, no children of this node or its sibilings will be Visited. + + All flavors of Visit methods have a default implementation that returns 'true' (continue + visiting). You need to only override methods that are interesting to you. + + Generally Accept() is called on the TiXmlDocument, although all nodes suppert Visiting. + + You should never change the document from a callback. + + @sa TiXmlNode::Accept() +*/ +class TiXmlVisitor +{ +public: + virtual ~TiXmlVisitor() {} + + /// Visit a document. + virtual bool VisitEnter( const TiXmlDocument& /*doc*/ ) { return true; } + /// Visit a document. + virtual bool VisitExit( const TiXmlDocument& /*doc*/ ) { return true; } + + /// Visit an element. + virtual bool VisitEnter( const TiXmlElement& /*element*/, const TiXmlAttribute* /*firstAttribute*/ ) { return true; } + /// Visit an element. + virtual bool VisitExit( const TiXmlElement& /*element*/ ) { return true; } + + /// Visit a declaration + virtual bool Visit( const TiXmlDeclaration& /*declaration*/ ) { return true; } + /// Visit a text node + virtual bool Visit( const TiXmlText& /*text*/ ) { return true; } + /// Visit a comment node + virtual bool Visit( const TiXmlComment& /*comment*/ ) { return true; } + /// Visit an unknown node + virtual bool Visit( const TiXmlUnknown& /*unknown*/ ) { return true; } +}; + +// Only used by Attribute::Query functions +enum +{ + TIXML_SUCCESS, + TIXML_NO_ATTRIBUTE, + TIXML_WRONG_TYPE +}; + + +// Used by the parsing routines. +enum TiXmlEncoding +{ + TIXML_ENCODING_UNKNOWN, + TIXML_ENCODING_UTF8, + TIXML_ENCODING_LEGACY +}; + +const TiXmlEncoding TIXML_DEFAULT_ENCODING = TIXML_ENCODING_UNKNOWN; + +/** TiXmlBase is a base class for every class in TinyXml. + It does little except to establish that TinyXml classes + can be printed and provide some utility functions. + + In XML, the document and elements can contain + other elements and other types of nodes. + + @verbatim + A Document can contain: Element (container or leaf) + Comment (leaf) + Unknown (leaf) + Declaration( leaf ) + + An Element can contain: Element (container or leaf) + Text (leaf) + Attributes (not on tree) + Comment (leaf) + Unknown (leaf) + + A Decleration contains: Attributes (not on tree) + @endverbatim +*/ +class TiXmlBase +{ + friend class TiXmlNode; + friend class TiXmlElement; + friend class TiXmlDocument; + +public: + TiXmlBase() : userData(0) {} + virtual ~TiXmlBase() {} + + /** All TinyXml classes can print themselves to a filestream + or the string class (TiXmlString in non-STL mode, std::string + in STL mode.) Either or both cfile and str can be null. + + This is a formatted print, and will insert + tabs and newlines. + + (For an unformatted stream, use the << operator.) + */ + virtual void Print( FILE* cfile, int depth ) const = 0; + + /** The world does not agree on whether white space should be kept or + not. In order to make everyone happy, these global, static functions + are provided to set whether or not TinyXml will condense all white space + into a single space or not. The default is to condense. Note changing this + value is not thread safe. + */ + static void SetCondenseWhiteSpace( bool condense ) { condenseWhiteSpace = condense; } + + /// Return the current white space setting. + static bool IsWhiteSpaceCondensed() { return condenseWhiteSpace; } + + /** Return the position, in the original source file, of this node or attribute. + The row and column are 1-based. (That is the first row and first column is + 1,1). If the returns values are 0 or less, then the parser does not have + a row and column value. + + Generally, the row and column value will be set when the TiXmlDocument::Load(), + TiXmlDocument::LoadFile(), or any TiXmlNode::Parse() is called. It will NOT be set + when the DOM was created from operator>>. + + The values reflect the initial load. Once the DOM is modified programmatically + (by adding or changing nodes and attributes) the new values will NOT update to + reflect changes in the document. + + There is a minor performance cost to computing the row and column. Computation + can be disabled if TiXmlDocument::SetTabSize() is called with 0 as the value. + + @sa TiXmlDocument::SetTabSize() + */ + int Row() const { return location.row + 1; } + int Column() const { return location.col + 1; } ///< See Row() + + void SetUserData( void* user ) { userData = user; } ///< Set a pointer to arbitrary user data. + void* GetUserData() { return userData; } ///< Get a pointer to arbitrary user data. + const void* GetUserData() const { return userData; } ///< Get a pointer to arbitrary user data. + + // Table that returs, for a given lead byte, the total number of bytes + // in the UTF-8 sequence. + static const int utf8ByteTable[256]; + + virtual const char* Parse( const char* p, + TiXmlParsingData* data, + TiXmlEncoding encoding /*= TIXML_ENCODING_UNKNOWN */ ) = 0; + + /** Expands entities in a string. Note this should not contian the tag's '<', '>', etc, + or they will be transformed into entities! + */ + static void EncodeString( const TIXML_STRING& str, TIXML_STRING* out ); + + enum + { + TIXML_NO_ERROR = 0, + TIXML_ERROR, + TIXML_ERROR_OPENING_FILE, + TIXML_ERROR_PARSING_ELEMENT, + TIXML_ERROR_FAILED_TO_READ_ELEMENT_NAME, + TIXML_ERROR_READING_ELEMENT_VALUE, + TIXML_ERROR_READING_ATTRIBUTES, + TIXML_ERROR_PARSING_EMPTY, + TIXML_ERROR_READING_END_TAG, + TIXML_ERROR_PARSING_UNKNOWN, + TIXML_ERROR_PARSING_COMMENT, + TIXML_ERROR_PARSING_DECLARATION, + TIXML_ERROR_DOCUMENT_EMPTY, + TIXML_ERROR_EMBEDDED_NULL, + TIXML_ERROR_PARSING_CDATA, + TIXML_ERROR_DOCUMENT_TOP_ONLY, + + TIXML_ERROR_STRING_COUNT + }; + +protected: + + static const char* SkipWhiteSpace( const char*, TiXmlEncoding encoding ); + + inline static bool IsWhiteSpace( char c ) + { + return ( isspace( (unsigned char) c ) || c == '\n' || c == '\r' ); + } + inline static bool IsWhiteSpace( int c ) + { + if ( c < 256 ) + return IsWhiteSpace( (char) c ); + return false; // Again, only truly correct for English/Latin...but usually works. + } + + #ifdef TIXML_USE_STL + static bool StreamWhiteSpace( std::istream * in, TIXML_STRING * tag ); + static bool StreamTo( std::istream * in, int character, TIXML_STRING * tag ); + #endif + + /* Reads an XML name into the string provided. Returns + a pointer just past the last character of the name, + or 0 if the function has an error. + */ + static const char* ReadName( const char* p, TIXML_STRING* name, TiXmlEncoding encoding ); + + /* Reads text. Returns a pointer past the given end tag. + Wickedly complex options, but it keeps the (sensitive) code in one place. + */ + static const char* ReadText( const char* in, // where to start + TIXML_STRING* text, // the string read + bool ignoreWhiteSpace, // whether to keep the white space + const char* endTag, // what ends this text + bool ignoreCase, // whether to ignore case in the end tag + TiXmlEncoding encoding ); // the current encoding + + // If an entity has been found, transform it into a character. + static const char* GetEntity( const char* in, char* value, int* length, TiXmlEncoding encoding ); + + // Get a character, while interpreting entities. + // The length can be from 0 to 4 bytes. + inline static const char* GetChar( const char* p, char* _value, int* length, TiXmlEncoding encoding ) + { + assert( p ); + if ( encoding == TIXML_ENCODING_UTF8 ) + { + *length = utf8ByteTable[ *((const unsigned char*)p) ]; + assert( *length >= 0 && *length < 5 ); + } + else + { + *length = 1; + } + + if ( *length == 1 ) + { + if ( *p == '&' ) + return GetEntity( p, _value, length, encoding ); + *_value = *p; + return p+1; + } + else if ( *length ) + { + //strncpy( _value, p, *length ); // lots of compilers don't like this function (unsafe), + // and the null terminator isn't needed + for( int i=0; p[i] && i<*length; ++i ) { + _value[i] = p[i]; + } + return p + (*length); + } + else + { + // Not valid text. + return 0; + } + } + + // Return true if the next characters in the stream are any of the endTag sequences. + // Ignore case only works for english, and should only be relied on when comparing + // to English words: StringEqual( p, "version", true ) is fine. + static bool StringEqual( const char* p, + const char* endTag, + bool ignoreCase, + TiXmlEncoding encoding ); + + static const char* errorString[ TIXML_ERROR_STRING_COUNT ]; + + TiXmlCursor location; + + /// Field containing a generic user pointer + void* userData; + + // None of these methods are reliable for any language except English. + // Good for approximation, not great for accuracy. + static int IsAlpha( unsigned char anyByte, TiXmlEncoding encoding ); + static int IsAlphaNum( unsigned char anyByte, TiXmlEncoding encoding ); + inline static int ToLower( int v, TiXmlEncoding encoding ) + { + if ( encoding == TIXML_ENCODING_UTF8 ) + { + if ( v < 128 ) return tolower( v ); + return v; + } + else + { + return tolower( v ); + } + } + static void ConvertUTF32ToUTF8( unsigned long input, char* output, int* length ); + +private: + TiXmlBase( const TiXmlBase& ); // not implemented. + void operator=( const TiXmlBase& base ); // not allowed. + + struct Entity + { + const char* str; + unsigned int strLength; + char chr; + }; + enum + { + NUM_ENTITY = 5, + MAX_ENTITY_LENGTH = 6 + + }; + static Entity entity[ NUM_ENTITY ]; + static bool condenseWhiteSpace; +}; + + +/** The parent class for everything in the Document Object Model. + (Except for attributes). + Nodes have siblings, a parent, and children. A node can be + in a document, or stand on its own. The type of a TiXmlNode + can be queried, and it can be cast to its more defined type. +*/ +class TiXmlNode : public TiXmlBase +{ + friend class TiXmlDocument; + friend class TiXmlElement; + +public: + #ifdef TIXML_USE_STL + + /** An input stream operator, for every class. Tolerant of newlines and + formatting, but doesn't expect them. + */ + friend std::istream& operator >> (std::istream& in, TiXmlNode& base); + + /** An output stream operator, for every class. Note that this outputs + without any newlines or formatting, as opposed to Print(), which + includes tabs and new lines. + + The operator<< and operator>> are not completely symmetric. Writing + a node to a stream is very well defined. You'll get a nice stream + of output, without any extra whitespace or newlines. + + But reading is not as well defined. (As it always is.) If you create + a TiXmlElement (for example) and read that from an input stream, + the text needs to define an element or junk will result. This is + true of all input streams, but it's worth keeping in mind. + + A TiXmlDocument will read nodes until it reads a root element, and + all the children of that root element. + */ + friend std::ostream& operator<< (std::ostream& out, const TiXmlNode& base); + + /// Appends the XML node or attribute to a std::string. + friend std::string& operator<< (std::string& out, const TiXmlNode& base ); + + #endif + + /** The types of XML nodes supported by TinyXml. (All the + unsupported types are picked up by UNKNOWN.) + */ + enum NodeType + { + TINYXML_DOCUMENT, + TINYXML_ELEMENT, + TINYXML_COMMENT, + TINYXML_UNKNOWN, + TINYXML_TEXT, + TINYXML_DECLARATION, + TINYXML_TYPECOUNT + }; + + virtual ~TiXmlNode(); + + /** The meaning of 'value' changes for the specific type of + TiXmlNode. + @verbatim + Document: filename of the xml file + Element: name of the element + Comment: the comment text + Unknown: the tag contents + Text: the text string + @endverbatim + + The subclasses will wrap this function. + */ + const char *Value() const { return value.c_str (); } + + #ifdef TIXML_USE_STL + /** Return Value() as a std::string. If you only use STL, + this is more efficient than calling Value(). + Only available in STL mode. + */ + const std::string& ValueStr() const { return value; } + #endif + + const TIXML_STRING& ValueTStr() const { return value; } + + /** Changes the value of the node. Defined as: + @verbatim + Document: filename of the xml file + Element: name of the element + Comment: the comment text + Unknown: the tag contents + Text: the text string + @endverbatim + */ + void SetValue(const char * _value) { value = _value;} + + #ifdef TIXML_USE_STL + /// STL std::string form. + void SetValue( const std::string& _value ) { value = _value; } + #endif + + /// Delete all the children of this node. Does not affect 'this'. + void Clear(); + + /// One step up the DOM. + TiXmlNode* Parent() { return parent; } + const TiXmlNode* Parent() const { return parent; } + + const TiXmlNode* FirstChild() const { return firstChild; } ///< The first child of this node. Will be null if there are no children. + TiXmlNode* FirstChild() { return firstChild; } + const TiXmlNode* FirstChild( const char * value ) const; ///< The first child of this node with the matching 'value'. Will be null if none found. + /// The first child of this node with the matching 'value'. Will be null if none found. + TiXmlNode* FirstChild( const char * _value ) { + // Call through to the const version - safe since nothing is changed. Exiting syntax: cast this to a const (always safe) + // call the method, cast the return back to non-const. + return const_cast< TiXmlNode* > ((const_cast< const TiXmlNode* >(this))->FirstChild( _value )); + } + const TiXmlNode* LastChild() const { return lastChild; } /// The last child of this node. Will be null if there are no children. + TiXmlNode* LastChild() { return lastChild; } + + const TiXmlNode* LastChild( const char * value ) const; /// The last child of this node matching 'value'. Will be null if there are no children. + TiXmlNode* LastChild( const char * _value ) { + return const_cast< TiXmlNode* > ((const_cast< const TiXmlNode* >(this))->LastChild( _value )); + } + + #ifdef TIXML_USE_STL + const TiXmlNode* FirstChild( const std::string& _value ) const { return FirstChild (_value.c_str ()); } ///< STL std::string form. + TiXmlNode* FirstChild( const std::string& _value ) { return FirstChild (_value.c_str ()); } ///< STL std::string form. + const TiXmlNode* LastChild( const std::string& _value ) const { return LastChild (_value.c_str ()); } ///< STL std::string form. + TiXmlNode* LastChild( const std::string& _value ) { return LastChild (_value.c_str ()); } ///< STL std::string form. + #endif + + /** An alternate way to walk the children of a node. + One way to iterate over nodes is: + @verbatim + for( child = parent->FirstChild(); child; child = child->NextSibling() ) + @endverbatim + + IterateChildren does the same thing with the syntax: + @verbatim + child = 0; + while( child = parent->IterateChildren( child ) ) + @endverbatim + + IterateChildren takes the previous child as input and finds + the next one. If the previous child is null, it returns the + first. IterateChildren will return null when done. + */ + const TiXmlNode* IterateChildren( const TiXmlNode* previous ) const; + TiXmlNode* IterateChildren( const TiXmlNode* previous ) { + return const_cast< TiXmlNode* >( (const_cast< const TiXmlNode* >(this))->IterateChildren( previous ) ); + } + + /// This flavor of IterateChildren searches for children with a particular 'value' + const TiXmlNode* IterateChildren( const char * value, const TiXmlNode* previous ) const; + TiXmlNode* IterateChildren( const char * _value, const TiXmlNode* previous ) { + return const_cast< TiXmlNode* >( (const_cast< const TiXmlNode* >(this))->IterateChildren( _value, previous ) ); + } + + #ifdef TIXML_USE_STL + const TiXmlNode* IterateChildren( const std::string& _value, const TiXmlNode* previous ) const { return IterateChildren (_value.c_str (), previous); } ///< STL std::string form. + TiXmlNode* IterateChildren( const std::string& _value, const TiXmlNode* previous ) { return IterateChildren (_value.c_str (), previous); } ///< STL std::string form. + #endif + + /** Add a new node related to this. Adds a child past the LastChild. + Returns a pointer to the new object or NULL if an error occured. + */ + TiXmlNode* InsertEndChild( const TiXmlNode& addThis ); + + + /** Add a new node related to this. Adds a child past the LastChild. + + NOTE: the node to be added is passed by pointer, and will be + henceforth owned (and deleted) by tinyXml. This method is efficient + and avoids an extra copy, but should be used with care as it + uses a different memory model than the other insert functions. + + @sa InsertEndChild + */ + TiXmlNode* LinkEndChild( TiXmlNode* addThis ); + + /** Add a new node related to this. Adds a child before the specified child. + Returns a pointer to the new object or NULL if an error occured. + */ + TiXmlNode* InsertBeforeChild( TiXmlNode* beforeThis, const TiXmlNode& addThis ); + + /** Add a new node related to this. Adds a child after the specified child. + Returns a pointer to the new object or NULL if an error occured. + */ + TiXmlNode* InsertAfterChild( TiXmlNode* afterThis, const TiXmlNode& addThis ); + + /** Replace a child of this node. + Returns a pointer to the new object or NULL if an error occured. + */ + TiXmlNode* ReplaceChild( TiXmlNode* replaceThis, const TiXmlNode& withThis ); + + /// Delete a child of this node. + bool RemoveChild( TiXmlNode* removeThis ); + + /// Navigate to a sibling node. + const TiXmlNode* PreviousSibling() const { return prev; } + TiXmlNode* PreviousSibling() { return prev; } + + /// Navigate to a sibling node. + const TiXmlNode* PreviousSibling( const char * ) const; + TiXmlNode* PreviousSibling( const char *_prev ) { + return const_cast< TiXmlNode* >( (const_cast< const TiXmlNode* >(this))->PreviousSibling( _prev ) ); + } + + #ifdef TIXML_USE_STL + const TiXmlNode* PreviousSibling( const std::string& _value ) const { return PreviousSibling (_value.c_str ()); } ///< STL std::string form. + TiXmlNode* PreviousSibling( const std::string& _value ) { return PreviousSibling (_value.c_str ()); } ///< STL std::string form. + const TiXmlNode* NextSibling( const std::string& _value) const { return NextSibling (_value.c_str ()); } ///< STL std::string form. + TiXmlNode* NextSibling( const std::string& _value) { return NextSibling (_value.c_str ()); } ///< STL std::string form. + #endif + + /// Navigate to a sibling node. + const TiXmlNode* NextSibling() const { return next; } + TiXmlNode* NextSibling() { return next; } + + /// Navigate to a sibling node with the given 'value'. + const TiXmlNode* NextSibling( const char * ) const; + TiXmlNode* NextSibling( const char* _next ) { + return const_cast< TiXmlNode* >( (const_cast< const TiXmlNode* >(this))->NextSibling( _next ) ); + } + + /** Convenience function to get through elements. + Calls NextSibling and ToElement. Will skip all non-Element + nodes. Returns 0 if there is not another element. + */ + const TiXmlElement* NextSiblingElement() const; + TiXmlElement* NextSiblingElement() { + return const_cast< TiXmlElement* >( (const_cast< const TiXmlNode* >(this))->NextSiblingElement() ); + } + + /** Convenience function to get through elements. + Calls NextSibling and ToElement. Will skip all non-Element + nodes. Returns 0 if there is not another element. + */ + const TiXmlElement* NextSiblingElement( const char * ) const; + TiXmlElement* NextSiblingElement( const char *_next ) { + return const_cast< TiXmlElement* >( (const_cast< const TiXmlNode* >(this))->NextSiblingElement( _next ) ); + } + + #ifdef TIXML_USE_STL + const TiXmlElement* NextSiblingElement( const std::string& _value) const { return NextSiblingElement (_value.c_str ()); } ///< STL std::string form. + TiXmlElement* NextSiblingElement( const std::string& _value) { return NextSiblingElement (_value.c_str ()); } ///< STL std::string form. + #endif + + /// Convenience function to get through elements. + const TiXmlElement* FirstChildElement() const; + TiXmlElement* FirstChildElement() { + return const_cast< TiXmlElement* >( (const_cast< const TiXmlNode* >(this))->FirstChildElement() ); + } + + /// Convenience function to get through elements. + const TiXmlElement* FirstChildElement( const char * _value ) const; + TiXmlElement* FirstChildElement( const char * _value ) { + return const_cast< TiXmlElement* >( (const_cast< const TiXmlNode* >(this))->FirstChildElement( _value ) ); + } + + #ifdef TIXML_USE_STL + const TiXmlElement* FirstChildElement( const std::string& _value ) const { return FirstChildElement (_value.c_str ()); } ///< STL std::string form. + TiXmlElement* FirstChildElement( const std::string& _value ) { return FirstChildElement (_value.c_str ()); } ///< STL std::string form. + #endif + + /** Query the type (as an enumerated value, above) of this node. + The possible types are: TINYXML_DOCUMENT, TINYXML_ELEMENT, TINYXML_COMMENT, + TINYXML_UNKNOWN, TINYXML_TEXT, and TINYXML_DECLARATION. + */ + int Type() const { return type; } + + /** Return a pointer to the Document this node lives in. + Returns null if not in a document. + */ + const TiXmlDocument* GetDocument() const; + TiXmlDocument* GetDocument() { + return const_cast< TiXmlDocument* >( (const_cast< const TiXmlNode* >(this))->GetDocument() ); + } + + /// Returns true if this node has no children. + bool NoChildren() const { return !firstChild; } + + virtual const TiXmlDocument* ToDocument() const { return 0; } ///< Cast to a more defined type. Will return null if not of the requested type. + virtual const TiXmlElement* ToElement() const { return 0; } ///< Cast to a more defined type. Will return null if not of the requested type. + virtual const TiXmlComment* ToComment() const { return 0; } ///< Cast to a more defined type. Will return null if not of the requested type. + virtual const TiXmlUnknown* ToUnknown() const { return 0; } ///< Cast to a more defined type. Will return null if not of the requested type. + virtual const TiXmlText* ToText() const { return 0; } ///< Cast to a more defined type. Will return null if not of the requested type. + virtual const TiXmlDeclaration* ToDeclaration() const { return 0; } ///< Cast to a more defined type. Will return null if not of the requested type. + + virtual TiXmlDocument* ToDocument() { return 0; } ///< Cast to a more defined type. Will return null if not of the requested type. + virtual TiXmlElement* ToElement() { return 0; } ///< Cast to a more defined type. Will return null if not of the requested type. + virtual TiXmlComment* ToComment() { return 0; } ///< Cast to a more defined type. Will return null if not of the requested type. + virtual TiXmlUnknown* ToUnknown() { return 0; } ///< Cast to a more defined type. Will return null if not of the requested type. + virtual TiXmlText* ToText() { return 0; } ///< Cast to a more defined type. Will return null if not of the requested type. + virtual TiXmlDeclaration* ToDeclaration() { return 0; } ///< Cast to a more defined type. Will return null if not of the requested type. + + /** Create an exact duplicate of this node and return it. The memory must be deleted + by the caller. + */ + virtual TiXmlNode* Clone() const = 0; + + /** Accept a hierchical visit the nodes in the TinyXML DOM. Every node in the + XML tree will be conditionally visited and the host will be called back + via the TiXmlVisitor interface. + + This is essentially a SAX interface for TinyXML. (Note however it doesn't re-parse + the XML for the callbacks, so the performance of TinyXML is unchanged by using this + interface versus any other.) + + The interface has been based on ideas from: + + - http://www.saxproject.org/ + - http://c2.com/cgi/wiki?HierarchicalVisitorPattern + + Which are both good references for "visiting". + + An example of using Accept(): + @verbatim + TiXmlPrinter printer; + tinyxmlDoc.Accept( &printer ); + const char* xmlcstr = printer.CStr(); + @endverbatim + */ + virtual bool Accept( TiXmlVisitor* visitor ) const = 0; + +protected: + TiXmlNode( NodeType _type ); + + // Copy to the allocated object. Shared functionality between Clone, Copy constructor, + // and the assignment operator. + void CopyTo( TiXmlNode* target ) const; + + #ifdef TIXML_USE_STL + // The real work of the input operator. + virtual void StreamIn( std::istream* in, TIXML_STRING* tag ) = 0; + #endif + + // Figure out what is at *p, and parse it. Returns null if it is not an xml node. + TiXmlNode* Identify( const char* start, TiXmlEncoding encoding ); + + TiXmlNode* parent; + NodeType type; + + TiXmlNode* firstChild; + TiXmlNode* lastChild; + + TIXML_STRING value; + + TiXmlNode* prev; + TiXmlNode* next; + +private: + TiXmlNode( const TiXmlNode& ); // not implemented. + void operator=( const TiXmlNode& base ); // not allowed. +}; + + +/** An attribute is a name-value pair. Elements have an arbitrary + number of attributes, each with a unique name. + + @note The attributes are not TiXmlNodes, since they are not + part of the tinyXML document object model. There are other + suggested ways to look at this problem. +*/ +class TiXmlAttribute : public TiXmlBase +{ + friend class TiXmlAttributeSet; + +public: + /// Construct an empty attribute. + TiXmlAttribute() : TiXmlBase() + { + document = 0; + prev = next = 0; + } + + #ifdef TIXML_USE_STL + /// std::string constructor. + TiXmlAttribute( const std::string& _name, const std::string& _value ) + { + name = _name; + value = _value; + document = 0; + prev = next = 0; + } + #endif + + /// Construct an attribute with a name and value. + TiXmlAttribute( const char * _name, const char * _value ) + { + name = _name; + value = _value; + document = 0; + prev = next = 0; + } + + const char* Name() const { return name.c_str(); } ///< Return the name of this attribute. + const char* Value() const { return value.c_str(); } ///< Return the value of this attribute. + #ifdef TIXML_USE_STL + const std::string& ValueStr() const { return value; } ///< Return the value of this attribute. + #endif + int IntValue() const; ///< Return the value of this attribute, converted to an integer. + double DoubleValue() const; ///< Return the value of this attribute, converted to a double. + + // Get the tinyxml string representation + const TIXML_STRING& NameTStr() const { return name; } + + /** QueryIntValue examines the value string. It is an alternative to the + IntValue() method with richer error checking. + If the value is an integer, it is stored in 'value' and + the call returns TIXML_SUCCESS. If it is not + an integer, it returns TIXML_WRONG_TYPE. + + A specialized but useful call. Note that for success it returns 0, + which is the opposite of almost all other TinyXml calls. + */ + int QueryIntValue( int* _value ) const; + /// QueryDoubleValue examines the value string. See QueryIntValue(). + int QueryDoubleValue( double* _value ) const; + + void SetName( const char* _name ) { name = _name; } ///< Set the name of this attribute. + void SetValue( const char* _value ) { value = _value; } ///< Set the value. + + void SetIntValue( int _value ); ///< Set the value from an integer. + void SetDoubleValue( double _value ); ///< Set the value from a double. + + #ifdef TIXML_USE_STL + /// STL std::string form. + void SetName( const std::string& _name ) { name = _name; } + /// STL std::string form. + void SetValue( const std::string& _value ) { value = _value; } + #endif + + /// Get the next sibling attribute in the DOM. Returns null at end. + const TiXmlAttribute* Next() const; + TiXmlAttribute* Next() { + return const_cast< TiXmlAttribute* >( (const_cast< const TiXmlAttribute* >(this))->Next() ); + } + + /// Get the previous sibling attribute in the DOM. Returns null at beginning. + const TiXmlAttribute* Previous() const; + TiXmlAttribute* Previous() { + return const_cast< TiXmlAttribute* >( (const_cast< const TiXmlAttribute* >(this))->Previous() ); + } + + bool operator==( const TiXmlAttribute& rhs ) const { return rhs.name == name; } + bool operator<( const TiXmlAttribute& rhs ) const { return name < rhs.name; } + bool operator>( const TiXmlAttribute& rhs ) const { return name > rhs.name; } + + /* Attribute parsing starts: first letter of the name + returns: the next char after the value end quote + */ + virtual const char* Parse( const char* p, TiXmlParsingData* data, TiXmlEncoding encoding ); + + // Prints this Attribute to a FILE stream. + virtual void Print( FILE* cfile, int depth ) const { + Print( cfile, depth, 0 ); + } + void Print( FILE* cfile, int depth, TIXML_STRING* str ) const; + + // [internal use] + // Set the document pointer so the attribute can report errors. + void SetDocument( TiXmlDocument* doc ) { document = doc; } + +private: + TiXmlAttribute( const TiXmlAttribute& ); // not implemented. + void operator=( const TiXmlAttribute& base ); // not allowed. + + TiXmlDocument* document; // A pointer back to a document, for error reporting. + TIXML_STRING name; + TIXML_STRING value; + TiXmlAttribute* prev; + TiXmlAttribute* next; +}; + + +/* A class used to manage a group of attributes. + It is only used internally, both by the ELEMENT and the DECLARATION. + + The set can be changed transparent to the Element and Declaration + classes that use it, but NOT transparent to the Attribute + which has to implement a next() and previous() method. Which makes + it a bit problematic and prevents the use of STL. + + This version is implemented with circular lists because: + - I like circular lists + - it demonstrates some independence from the (typical) doubly linked list. +*/ +class TiXmlAttributeSet +{ +public: + TiXmlAttributeSet(); + ~TiXmlAttributeSet(); + + void Add( TiXmlAttribute* attribute ); + void Remove( TiXmlAttribute* attribute ); + + const TiXmlAttribute* First() const { return ( sentinel.next == &sentinel ) ? 0 : sentinel.next; } + TiXmlAttribute* First() { return ( sentinel.next == &sentinel ) ? 0 : sentinel.next; } + const TiXmlAttribute* Last() const { return ( sentinel.prev == &sentinel ) ? 0 : sentinel.prev; } + TiXmlAttribute* Last() { return ( sentinel.prev == &sentinel ) ? 0 : sentinel.prev; } + + TiXmlAttribute* Find( const char* _name ) const; + TiXmlAttribute* FindOrCreate( const char* _name ); + +# ifdef TIXML_USE_STL + TiXmlAttribute* Find( const std::string& _name ) const; + TiXmlAttribute* FindOrCreate( const std::string& _name ); +# endif + + +private: + //*ME: Because of hidden/disabled copy-construktor in TiXmlAttribute (sentinel-element), + //*ME: this class must be also use a hidden/disabled copy-constructor !!! + TiXmlAttributeSet( const TiXmlAttributeSet& ); // not allowed + void operator=( const TiXmlAttributeSet& ); // not allowed (as TiXmlAttribute) + + TiXmlAttribute sentinel; +}; + + +/** The element is a container class. It has a value, the element name, + and can contain other elements, text, comments, and unknowns. + Elements also contain an arbitrary number of attributes. +*/ +class TiXmlElement : public TiXmlNode +{ +public: + /// Construct an element. + TiXmlElement (const char * in_value); + + #ifdef TIXML_USE_STL + /// std::string constructor. + TiXmlElement( const std::string& _value ); + #endif + + TiXmlElement( const TiXmlElement& ); + + TiXmlElement& operator=( const TiXmlElement& base ); + + virtual ~TiXmlElement(); + + /** Given an attribute name, Attribute() returns the value + for the attribute of that name, or null if none exists. + */ + const char* Attribute( const char* name ) const; + + /** Given an attribute name, Attribute() returns the value + for the attribute of that name, or null if none exists. + If the attribute exists and can be converted to an integer, + the integer value will be put in the return 'i', if 'i' + is non-null. + */ + const char* Attribute( const char* name, int* i ) const; + + /** Given an attribute name, Attribute() returns the value + for the attribute of that name, or null if none exists. + If the attribute exists and can be converted to an double, + the double value will be put in the return 'd', if 'd' + is non-null. + */ + const char* Attribute( const char* name, double* d ) const; + + /** QueryIntAttribute examines the attribute - it is an alternative to the + Attribute() method with richer error checking. + If the attribute is an integer, it is stored in 'value' and + the call returns TIXML_SUCCESS. If it is not + an integer, it returns TIXML_WRONG_TYPE. If the attribute + does not exist, then TIXML_NO_ATTRIBUTE is returned. + */ + int QueryIntAttribute( const char* name, int* _value ) const; + /// QueryUnsignedAttribute examines the attribute - see QueryIntAttribute(). + int QueryUnsignedAttribute( const char* name, unsigned* _value ) const; + /** QueryBoolAttribute examines the attribute - see QueryIntAttribute(). + Note that '1', 'true', or 'yes' are considered true, while '0', 'false' + and 'no' are considered false. + */ + int QueryBoolAttribute( const char* name, bool* _value ) const; + /// QueryDoubleAttribute examines the attribute - see QueryIntAttribute(). + int QueryDoubleAttribute( const char* name, double* _value ) const; + /// QueryFloatAttribute examines the attribute - see QueryIntAttribute(). + int QueryFloatAttribute( const char* name, float* _value ) const { + double d; + int result = QueryDoubleAttribute( name, &d ); + if ( result == TIXML_SUCCESS ) { + *_value = (float)d; + } + return result; + } + + #ifdef TIXML_USE_STL + /// QueryStringAttribute examines the attribute - see QueryIntAttribute(). + int QueryStringAttribute( const char* name, std::string* _value ) const { + const char* cstr = Attribute( name ); + if ( cstr ) { + *_value = std::string( cstr ); + return TIXML_SUCCESS; + } + return TIXML_NO_ATTRIBUTE; + } + + /** Template form of the attribute query which will try to read the + attribute into the specified type. Very easy, very powerful, but + be careful to make sure to call this with the correct type. + + NOTE: This method doesn't work correctly for 'string' types that contain spaces. + + @return TIXML_SUCCESS, TIXML_WRONG_TYPE, or TIXML_NO_ATTRIBUTE + */ + template< typename T > int QueryValueAttribute( const std::string& name, T* outValue ) const + { + const TiXmlAttribute* node = attributeSet.Find( name ); + if ( !node ) + return TIXML_NO_ATTRIBUTE; + + std::stringstream sstream( node->ValueStr() ); + sstream >> *outValue; + if ( !sstream.fail() ) + return TIXML_SUCCESS; + return TIXML_WRONG_TYPE; + } + + int QueryValueAttribute( const std::string& name, std::string* outValue ) const + { + const TiXmlAttribute* node = attributeSet.Find( name ); + if ( !node ) + return TIXML_NO_ATTRIBUTE; + *outValue = node->ValueStr(); + return TIXML_SUCCESS; + } + #endif + + /** Sets an attribute of name to a given value. The attribute + will be created if it does not exist, or changed if it does. + */ + void SetAttribute( const char* name, const char * _value ); + + #ifdef TIXML_USE_STL + const std::string* Attribute( const std::string& name ) const; + const std::string* Attribute( const std::string& name, int* i ) const; + const std::string* Attribute( const std::string& name, double* d ) const; + int QueryIntAttribute( const std::string& name, int* _value ) const; + int QueryDoubleAttribute( const std::string& name, double* _value ) const; + + /// STL std::string form. + void SetAttribute( const std::string& name, const std::string& _value ); + ///< STL std::string form. + void SetAttribute( const std::string& name, int _value ); + ///< STL std::string form. + void SetDoubleAttribute( const std::string& name, double value ); + #endif + + /** Sets an attribute of name to a given value. The attribute + will be created if it does not exist, or changed if it does. + */ + void SetAttribute( const char * name, int value ); + + /** Sets an attribute of name to a given value. The attribute + will be created if it does not exist, or changed if it does. + */ + void SetDoubleAttribute( const char * name, double value ); + + /** Deletes an attribute with the given name. + */ + void RemoveAttribute( const char * name ); + #ifdef TIXML_USE_STL + void RemoveAttribute( const std::string& name ) { RemoveAttribute (name.c_str ()); } ///< STL std::string form. + #endif + + const TiXmlAttribute* FirstAttribute() const { return attributeSet.First(); } ///< Access the first attribute in this element. + TiXmlAttribute* FirstAttribute() { return attributeSet.First(); } + const TiXmlAttribute* LastAttribute() const { return attributeSet.Last(); } ///< Access the last attribute in this element. + TiXmlAttribute* LastAttribute() { return attributeSet.Last(); } + + /** Convenience function for easy access to the text inside an element. Although easy + and concise, GetText() is limited compared to getting the TiXmlText child + and accessing it directly. + + If the first child of 'this' is a TiXmlText, the GetText() + returns the character string of the Text node, else null is returned. + + This is a convenient method for getting the text of simple contained text: + @verbatim + This is text + const char* str = fooElement->GetText(); + @endverbatim + + 'str' will be a pointer to "This is text". + + Note that this function can be misleading. If the element foo was created from + this XML: + @verbatim + This is text + @endverbatim + + then the value of str would be null. The first child node isn't a text node, it is + another element. From this XML: + @verbatim + This is text + @endverbatim + GetText() will return "This is ". + + WARNING: GetText() accesses a child node - don't become confused with the + similarly named TiXmlHandle::Text() and TiXmlNode::ToText() which are + safe type casts on the referenced node. + */ + const char* GetText() const; + + /// Creates a new Element and returns it - the returned element is a copy. + virtual TiXmlNode* Clone() const; + // Print the Element to a FILE stream. + virtual void Print( FILE* cfile, int depth ) const; + + /* Attribtue parsing starts: next char past '<' + returns: next char past '>' + */ + virtual const char* Parse( const char* p, TiXmlParsingData* data, TiXmlEncoding encoding ); + + virtual const TiXmlElement* ToElement() const { return this; } ///< Cast to a more defined type. Will return null not of the requested type. + virtual TiXmlElement* ToElement() { return this; } ///< Cast to a more defined type. Will return null not of the requested type. + + /** Walk the XML tree visiting this node and all of its children. + */ + virtual bool Accept( TiXmlVisitor* visitor ) const; + +protected: + + void CopyTo( TiXmlElement* target ) const; + void ClearThis(); // like clear, but initializes 'this' object as well + + // Used to be public [internal use] + #ifdef TIXML_USE_STL + virtual void StreamIn( std::istream * in, TIXML_STRING * tag ); + #endif + /* [internal use] + Reads the "value" of the element -- another element, or text. + This should terminate with the current end tag. + */ + const char* ReadValue( const char* in, TiXmlParsingData* prevData, TiXmlEncoding encoding ); + +private: + TiXmlAttributeSet attributeSet; +}; + + +/** An XML comment. +*/ +class TiXmlComment : public TiXmlNode +{ +public: + /// Constructs an empty comment. + TiXmlComment() : TiXmlNode( TiXmlNode::TINYXML_COMMENT ) {} + /// Construct a comment from text. + TiXmlComment( const char* _value ) : TiXmlNode( TiXmlNode::TINYXML_COMMENT ) { + SetValue( _value ); + } + TiXmlComment( const TiXmlComment& ); + TiXmlComment& operator=( const TiXmlComment& base ); + + virtual ~TiXmlComment() {} + + /// Returns a copy of this Comment. + virtual TiXmlNode* Clone() const; + // Write this Comment to a FILE stream. + virtual void Print( FILE* cfile, int depth ) const; + + /* Attribtue parsing starts: at the ! of the !-- + returns: next char past '>' + */ + virtual const char* Parse( const char* p, TiXmlParsingData* data, TiXmlEncoding encoding ); + + virtual const TiXmlComment* ToComment() const { return this; } ///< Cast to a more defined type. Will return null not of the requested type. + virtual TiXmlComment* ToComment() { return this; } ///< Cast to a more defined type. Will return null not of the requested type. + + /** Walk the XML tree visiting this node and all of its children. + */ + virtual bool Accept( TiXmlVisitor* visitor ) const; + +protected: + void CopyTo( TiXmlComment* target ) const; + + // used to be public + #ifdef TIXML_USE_STL + virtual void StreamIn( std::istream * in, TIXML_STRING * tag ); + #endif +// virtual void StreamOut( TIXML_OSTREAM * out ) const; + +private: + +}; + + +/** XML text. A text node can have 2 ways to output the next. "normal" output + and CDATA. It will default to the mode it was parsed from the XML file and + you generally want to leave it alone, but you can change the output mode with + SetCDATA() and query it with CDATA(). +*/ +class TiXmlText : public TiXmlNode +{ + friend class TiXmlElement; +public: + /** Constructor for text element. By default, it is treated as + normal, encoded text. If you want it be output as a CDATA text + element, set the parameter _cdata to 'true' + */ + TiXmlText (const char * initValue ) : TiXmlNode (TiXmlNode::TINYXML_TEXT) + { + SetValue( initValue ); + cdata = false; + } + virtual ~TiXmlText() {} + + #ifdef TIXML_USE_STL + /// Constructor. + TiXmlText( const std::string& initValue ) : TiXmlNode (TiXmlNode::TINYXML_TEXT) + { + SetValue( initValue ); + cdata = false; + } + #endif + + TiXmlText( const TiXmlText& copy ) : TiXmlNode( TiXmlNode::TINYXML_TEXT ) { copy.CopyTo( this ); } + TiXmlText& operator=( const TiXmlText& base ) { base.CopyTo( this ); return *this; } + + // Write this text object to a FILE stream. + virtual void Print( FILE* cfile, int depth ) const; + + /// Queries whether this represents text using a CDATA section. + bool CDATA() const { return cdata; } + /// Turns on or off a CDATA representation of text. + void SetCDATA( bool _cdata ) { cdata = _cdata; } + + virtual const char* Parse( const char* p, TiXmlParsingData* data, TiXmlEncoding encoding ); + + virtual const TiXmlText* ToText() const { return this; } ///< Cast to a more defined type. Will return null not of the requested type. + virtual TiXmlText* ToText() { return this; } ///< Cast to a more defined type. Will return null not of the requested type. + + /** Walk the XML tree visiting this node and all of its children. + */ + virtual bool Accept( TiXmlVisitor* content ) const; + +protected : + /// [internal use] Creates a new Element and returns it. + virtual TiXmlNode* Clone() const; + void CopyTo( TiXmlText* target ) const; + + bool Blank() const; // returns true if all white space and new lines + // [internal use] + #ifdef TIXML_USE_STL + virtual void StreamIn( std::istream * in, TIXML_STRING * tag ); + #endif + +private: + bool cdata; // true if this should be input and output as a CDATA style text element +}; + + +/** In correct XML the declaration is the first entry in the file. + @verbatim + + @endverbatim + + TinyXml will happily read or write files without a declaration, + however. There are 3 possible attributes to the declaration: + version, encoding, and standalone. + + Note: In this version of the code, the attributes are + handled as special cases, not generic attributes, simply + because there can only be at most 3 and they are always the same. +*/ +class TiXmlDeclaration : public TiXmlNode +{ +public: + /// Construct an empty declaration. + TiXmlDeclaration() : TiXmlNode( TiXmlNode::TINYXML_DECLARATION ) {} + +#ifdef TIXML_USE_STL + /// Constructor. + TiXmlDeclaration( const std::string& _version, + const std::string& _encoding, + const std::string& _standalone ); +#endif + + /// Construct. + TiXmlDeclaration( const char* _version, + const char* _encoding, + const char* _standalone ); + + TiXmlDeclaration( const TiXmlDeclaration& copy ); + TiXmlDeclaration& operator=( const TiXmlDeclaration& copy ); + + virtual ~TiXmlDeclaration() {} + + /// Version. Will return an empty string if none was found. + const char *Version() const { return version.c_str (); } + /// Encoding. Will return an empty string if none was found. + const char *Encoding() const { return encoding.c_str (); } + /// Is this a standalone document? + const char *Standalone() const { return standalone.c_str (); } + + /// Creates a copy of this Declaration and returns it. + virtual TiXmlNode* Clone() const; + // Print this declaration to a FILE stream. + virtual void Print( FILE* cfile, int depth, TIXML_STRING* str ) const; + virtual void Print( FILE* cfile, int depth ) const { + Print( cfile, depth, 0 ); + } + + virtual const char* Parse( const char* p, TiXmlParsingData* data, TiXmlEncoding encoding ); + + virtual const TiXmlDeclaration* ToDeclaration() const { return this; } ///< Cast to a more defined type. Will return null not of the requested type. + virtual TiXmlDeclaration* ToDeclaration() { return this; } ///< Cast to a more defined type. Will return null not of the requested type. + + /** Walk the XML tree visiting this node and all of its children. + */ + virtual bool Accept( TiXmlVisitor* visitor ) const; + +protected: + void CopyTo( TiXmlDeclaration* target ) const; + // used to be public + #ifdef TIXML_USE_STL + virtual void StreamIn( std::istream * in, TIXML_STRING * tag ); + #endif + +private: + + TIXML_STRING version; + TIXML_STRING encoding; + TIXML_STRING standalone; +}; + + +/** Any tag that tinyXml doesn't recognize is saved as an + unknown. It is a tag of text, but should not be modified. + It will be written back to the XML, unchanged, when the file + is saved. + + DTD tags get thrown into TiXmlUnknowns. +*/ +class TiXmlUnknown : public TiXmlNode +{ +public: + TiXmlUnknown() : TiXmlNode( TiXmlNode::TINYXML_UNKNOWN ) {} + virtual ~TiXmlUnknown() {} + + TiXmlUnknown( const TiXmlUnknown& copy ) : TiXmlNode( TiXmlNode::TINYXML_UNKNOWN ) { copy.CopyTo( this ); } + TiXmlUnknown& operator=( const TiXmlUnknown& copy ) { copy.CopyTo( this ); return *this; } + + /// Creates a copy of this Unknown and returns it. + virtual TiXmlNode* Clone() const; + // Print this Unknown to a FILE stream. + virtual void Print( FILE* cfile, int depth ) const; + + virtual const char* Parse( const char* p, TiXmlParsingData* data, TiXmlEncoding encoding ); + + virtual const TiXmlUnknown* ToUnknown() const { return this; } ///< Cast to a more defined type. Will return null not of the requested type. + virtual TiXmlUnknown* ToUnknown() { return this; } ///< Cast to a more defined type. Will return null not of the requested type. + + /** Walk the XML tree visiting this node and all of its children. + */ + virtual bool Accept( TiXmlVisitor* content ) const; + +protected: + void CopyTo( TiXmlUnknown* target ) const; + + #ifdef TIXML_USE_STL + virtual void StreamIn( std::istream * in, TIXML_STRING * tag ); + #endif + +private: + +}; + + +/** Always the top level node. A document binds together all the + XML pieces. It can be saved, loaded, and printed to the screen. + The 'value' of a document node is the xml file name. +*/ +class TiXmlDocument : public TiXmlNode +{ +public: + /// Create an empty document, that has no name. + TiXmlDocument(); + /// Create a document with a name. The name of the document is also the filename of the xml. + TiXmlDocument( const char * documentName ); + + #ifdef TIXML_USE_STL + /// Constructor. + TiXmlDocument( const std::string& documentName ); + #endif + + TiXmlDocument( const TiXmlDocument& copy ); + TiXmlDocument& operator=( const TiXmlDocument& copy ); + + virtual ~TiXmlDocument() {} + + /** Load a file using the current document value. + Returns true if successful. Will delete any existing + document data before loading. + */ + bool LoadFile( TiXmlEncoding encoding = TIXML_DEFAULT_ENCODING ); + /// Save a file using the current document value. Returns true if successful. + bool SaveFile() const; + /// Load a file using the given filename. Returns true if successful. + bool LoadFile( const char * filename, TiXmlEncoding encoding = TIXML_DEFAULT_ENCODING ); + /// Save a file using the given filename. Returns true if successful. + bool SaveFile( const char * filename ) const; + /** Load a file using the given FILE*. Returns true if successful. Note that this method + doesn't stream - the entire object pointed at by the FILE* + will be interpreted as an XML file. TinyXML doesn't stream in XML from the current + file location. Streaming may be added in the future. + */ + bool LoadFile( FILE*, TiXmlEncoding encoding = TIXML_DEFAULT_ENCODING ); + /// Save a file using the given FILE*. Returns true if successful. + bool SaveFile( FILE* ) const; + + #ifdef TIXML_USE_STL + bool LoadFile( const std::string& filename, TiXmlEncoding encoding = TIXML_DEFAULT_ENCODING ) ///< STL std::string version. + { + return LoadFile( filename.c_str(), encoding ); + } + bool SaveFile( const std::string& filename ) const ///< STL std::string version. + { + return SaveFile( filename.c_str() ); + } + #endif + + /** Parse the given null terminated block of xml data. Passing in an encoding to this + method (either TIXML_ENCODING_LEGACY or TIXML_ENCODING_UTF8 will force TinyXml + to use that encoding, regardless of what TinyXml might otherwise try to detect. + */ + virtual const char* Parse( const char* p, TiXmlParsingData* data = 0, TiXmlEncoding encoding = TIXML_DEFAULT_ENCODING ); + + /** Get the root element -- the only top level element -- of the document. + In well formed XML, there should only be one. TinyXml is tolerant of + multiple elements at the document level. + */ + const TiXmlElement* RootElement() const { return FirstChildElement(); } + TiXmlElement* RootElement() { return FirstChildElement(); } + + /** If an error occurs, Error will be set to true. Also, + - The ErrorId() will contain the integer identifier of the error (not generally useful) + - The ErrorDesc() method will return the name of the error. (very useful) + - The ErrorRow() and ErrorCol() will return the location of the error (if known) + */ + bool Error() const { return error; } + + /// Contains a textual (english) description of the error if one occurs. + const char * ErrorDesc() const { return errorDesc.c_str (); } + + /** Generally, you probably want the error string ( ErrorDesc() ). But if you + prefer the ErrorId, this function will fetch it. + */ + int ErrorId() const { return errorId; } + + /** Returns the location (if known) of the error. The first column is column 1, + and the first row is row 1. A value of 0 means the row and column wasn't applicable + (memory errors, for example, have no row/column) or the parser lost the error. (An + error in the error reporting, in that case.) + + @sa SetTabSize, Row, Column + */ + int ErrorRow() const { return errorLocation.row+1; } + int ErrorCol() const { return errorLocation.col+1; } ///< The column where the error occured. See ErrorRow() + + /** SetTabSize() allows the error reporting functions (ErrorRow() and ErrorCol()) + to report the correct values for row and column. It does not change the output + or input in any way. + + By calling this method, with a tab size + greater than 0, the row and column of each node and attribute is stored + when the file is loaded. Very useful for tracking the DOM back in to + the source file. + + The tab size is required for calculating the location of nodes. If not + set, the default of 4 is used. The tabsize is set per document. Setting + the tabsize to 0 disables row/column tracking. + + Note that row and column tracking is not supported when using operator>>. + + The tab size needs to be enabled before the parse or load. Correct usage: + @verbatim + TiXmlDocument doc; + doc.SetTabSize( 8 ); + doc.Load( "myfile.xml" ); + @endverbatim + + @sa Row, Column + */ + void SetTabSize( int _tabsize ) { tabsize = _tabsize; } + + int TabSize() const { return tabsize; } + + /** If you have handled the error, it can be reset with this call. The error + state is automatically cleared if you Parse a new XML block. + */ + void ClearError() { error = false; + errorId = 0; + errorDesc = ""; + errorLocation.row = errorLocation.col = 0; + //errorLocation.last = 0; + } + + /** Write the document to standard out using formatted printing ("pretty print"). */ + void Print() const { Print( stdout, 0 ); } + + /* Write the document to a string using formatted printing ("pretty print"). This + will allocate a character array (new char[]) and return it as a pointer. The + calling code pust call delete[] on the return char* to avoid a memory leak. + */ + //char* PrintToMemory() const; + + /// Print this Document to a FILE stream. + virtual void Print( FILE* cfile, int depth = 0 ) const; + // [internal use] + void SetError( int err, const char* errorLocation, TiXmlParsingData* prevData, TiXmlEncoding encoding ); + + virtual const TiXmlDocument* ToDocument() const { return this; } ///< Cast to a more defined type. Will return null not of the requested type. + virtual TiXmlDocument* ToDocument() { return this; } ///< Cast to a more defined type. Will return null not of the requested type. + + /** Walk the XML tree visiting this node and all of its children. + */ + virtual bool Accept( TiXmlVisitor* content ) const; + +protected : + // [internal use] + virtual TiXmlNode* Clone() const; + #ifdef TIXML_USE_STL + virtual void StreamIn( std::istream * in, TIXML_STRING * tag ); + #endif + +private: + void CopyTo( TiXmlDocument* target ) const; + + bool error; + int errorId; + TIXML_STRING errorDesc; + int tabsize; + TiXmlCursor errorLocation; + bool useMicrosoftBOM; // the UTF-8 BOM were found when read. Note this, and try to write. +}; + + +/** + A TiXmlHandle is a class that wraps a node pointer with null checks; this is + an incredibly useful thing. Note that TiXmlHandle is not part of the TinyXml + DOM structure. It is a separate utility class. + + Take an example: + @verbatim + + + + + + + @endverbatim + + Assuming you want the value of "attributeB" in the 2nd "Child" element, it's very + easy to write a *lot* of code that looks like: + + @verbatim + TiXmlElement* root = document.FirstChildElement( "Document" ); + if ( root ) + { + TiXmlElement* element = root->FirstChildElement( "Element" ); + if ( element ) + { + TiXmlElement* child = element->FirstChildElement( "Child" ); + if ( child ) + { + TiXmlElement* child2 = child->NextSiblingElement( "Child" ); + if ( child2 ) + { + // Finally do something useful. + @endverbatim + + And that doesn't even cover "else" cases. TiXmlHandle addresses the verbosity + of such code. A TiXmlHandle checks for null pointers so it is perfectly safe + and correct to use: + + @verbatim + TiXmlHandle docHandle( &document ); + TiXmlElement* child2 = docHandle.FirstChild( "Document" ).FirstChild( "Element" ).Child( "Child", 1 ).ToElement(); + if ( child2 ) + { + // do something useful + @endverbatim + + Which is MUCH more concise and useful. + + It is also safe to copy handles - internally they are nothing more than node pointers. + @verbatim + TiXmlHandle handleCopy = handle; + @endverbatim + + What they should not be used for is iteration: + + @verbatim + int i=0; + while ( true ) + { + TiXmlElement* child = docHandle.FirstChild( "Document" ).FirstChild( "Element" ).Child( "Child", i ).ToElement(); + if ( !child ) + break; + // do something + ++i; + } + @endverbatim + + It seems reasonable, but it is in fact two embedded while loops. The Child method is + a linear walk to find the element, so this code would iterate much more than it needs + to. Instead, prefer: + + @verbatim + TiXmlElement* child = docHandle.FirstChild( "Document" ).FirstChild( "Element" ).FirstChild( "Child" ).ToElement(); + + for( child; child; child=child->NextSiblingElement() ) + { + // do something + } + @endverbatim +*/ +class TiXmlHandle +{ +public: + /// Create a handle from any node (at any depth of the tree.) This can be a null pointer. + TiXmlHandle( TiXmlNode* _node ) { this->node = _node; } + /// Copy constructor + TiXmlHandle( const TiXmlHandle& ref ) { this->node = ref.node; } + TiXmlHandle operator=( const TiXmlHandle& ref ) { if ( &ref != this ) this->node = ref.node; return *this; } + + /// Return a handle to the first child node. + TiXmlHandle FirstChild() const; + /// Return a handle to the first child node with the given name. + TiXmlHandle FirstChild( const char * value ) const; + /// Return a handle to the first child element. + TiXmlHandle FirstChildElement() const; + /// Return a handle to the first child element with the given name. + TiXmlHandle FirstChildElement( const char * value ) const; + + /** Return a handle to the "index" child with the given name. + The first child is 0, the second 1, etc. + */ + TiXmlHandle Child( const char* value, int index ) const; + /** Return a handle to the "index" child. + The first child is 0, the second 1, etc. + */ + TiXmlHandle Child( int index ) const; + /** Return a handle to the "index" child element with the given name. + The first child element is 0, the second 1, etc. Note that only TiXmlElements + are indexed: other types are not counted. + */ + TiXmlHandle ChildElement( const char* value, int index ) const; + /** Return a handle to the "index" child element. + The first child element is 0, the second 1, etc. Note that only TiXmlElements + are indexed: other types are not counted. + */ + TiXmlHandle ChildElement( int index ) const; + + #ifdef TIXML_USE_STL + TiXmlHandle FirstChild( const std::string& _value ) const { return FirstChild( _value.c_str() ); } + TiXmlHandle FirstChildElement( const std::string& _value ) const { return FirstChildElement( _value.c_str() ); } + + TiXmlHandle Child( const std::string& _value, int index ) const { return Child( _value.c_str(), index ); } + TiXmlHandle ChildElement( const std::string& _value, int index ) const { return ChildElement( _value.c_str(), index ); } + #endif + + /** Return the handle as a TiXmlNode. This may return null. + */ + TiXmlNode* ToNode() const { return node; } + /** Return the handle as a TiXmlElement. This may return null. + */ + TiXmlElement* ToElement() const { return ( ( node && node->ToElement() ) ? node->ToElement() : 0 ); } + /** Return the handle as a TiXmlText. This may return null. + */ + TiXmlText* ToText() const { return ( ( node && node->ToText() ) ? node->ToText() : 0 ); } + /** Return the handle as a TiXmlUnknown. This may return null. + */ + TiXmlUnknown* ToUnknown() const { return ( ( node && node->ToUnknown() ) ? node->ToUnknown() : 0 ); } + + /** @deprecated use ToNode. + Return the handle as a TiXmlNode. This may return null. + */ + TiXmlNode* Node() const { return ToNode(); } + /** @deprecated use ToElement. + Return the handle as a TiXmlElement. This may return null. + */ + TiXmlElement* Element() const { return ToElement(); } + /** @deprecated use ToText() + Return the handle as a TiXmlText. This may return null. + */ + TiXmlText* Text() const { return ToText(); } + /** @deprecated use ToUnknown() + Return the handle as a TiXmlUnknown. This may return null. + */ + TiXmlUnknown* Unknown() const { return ToUnknown(); } + +private: + TiXmlNode* node; +}; + + +/** Print to memory functionality. The TiXmlPrinter is useful when you need to: + + -# Print to memory (especially in non-STL mode) + -# Control formatting (line endings, etc.) + + When constructed, the TiXmlPrinter is in its default "pretty printing" mode. + Before calling Accept() you can call methods to control the printing + of the XML document. After TiXmlNode::Accept() is called, the printed document can + be accessed via the CStr(), Str(), and Size() methods. + + TiXmlPrinter uses the Visitor API. + @verbatim + TiXmlPrinter printer; + printer.SetIndent( "\t" ); + + doc.Accept( &printer ); + fprintf( stdout, "%s", printer.CStr() ); + @endverbatim +*/ +class TiXmlPrinter : public TiXmlVisitor +{ +public: + TiXmlPrinter() : depth( 0 ), simpleTextPrint( false ), + buffer(), indent( " " ), lineBreak( "\n" ) {} + + virtual bool VisitEnter( const TiXmlDocument& doc ); + virtual bool VisitExit( const TiXmlDocument& doc ); + + virtual bool VisitEnter( const TiXmlElement& element, const TiXmlAttribute* firstAttribute ); + virtual bool VisitExit( const TiXmlElement& element ); + + virtual bool Visit( const TiXmlDeclaration& declaration ); + virtual bool Visit( const TiXmlText& text ); + virtual bool Visit( const TiXmlComment& comment ); + virtual bool Visit( const TiXmlUnknown& unknown ); + + /** Set the indent characters for printing. By default 4 spaces + but tab (\t) is also useful, or null/empty string for no indentation. + */ + void SetIndent( const char* _indent ) { indent = _indent ? _indent : "" ; } + /// Query the indention string. + const char* Indent() { return indent.c_str(); } + /** Set the line breaking string. By default set to newline (\n). + Some operating systems prefer other characters, or can be + set to the null/empty string for no indenation. + */ + void SetLineBreak( const char* _lineBreak ) { lineBreak = _lineBreak ? _lineBreak : ""; } + /// Query the current line breaking string. + const char* LineBreak() { return lineBreak.c_str(); } + + /** Switch over to "stream printing" which is the most dense formatting without + linebreaks. Common when the XML is needed for network transmission. + */ + void SetStreamPrinting() { indent = ""; + lineBreak = ""; + } + /// Return the result. + const char* CStr() { return buffer.c_str(); } + /// Return the length of the result string. + size_t Size() { return buffer.size(); } + + #ifdef TIXML_USE_STL + /// Return the result. + const std::string& Str() { return buffer; } + #endif + +private: + void DoIndent() { + for( int i=0; i(-1); + + +// Null rep. +TiXmlString::Rep TiXmlString::nullrep_ = { 0, 0, { '\0' } }; + + +void TiXmlString::reserve (size_type cap) +{ + if (cap > capacity()) + { + TiXmlString tmp; + tmp.init(length(), cap); + memcpy(tmp.start(), data(), length()); + swap(tmp); + } +} + + +TiXmlString& TiXmlString::assign(const char* str, size_type len) +{ + size_type cap = capacity(); + if (len > cap || cap > 3*(len + 8)) + { + TiXmlString tmp; + tmp.init(len); + memcpy(tmp.start(), str, len); + swap(tmp); + } + else + { + memmove(start(), str, len); + set_size(len); + } + return *this; +} + + +TiXmlString& TiXmlString::append(const char* str, size_type len) +{ + size_type newsize = length() + len; + if (newsize > capacity()) + { + reserve (newsize + capacity()); + } + memmove(finish(), str, len); + set_size(newsize); + return *this; +} + + +TiXmlString operator + (const TiXmlString & a, const TiXmlString & b) +{ + TiXmlString tmp; + tmp.reserve(a.length() + b.length()); + tmp += a; + tmp += b; + return tmp; +} + +TiXmlString operator + (const TiXmlString & a, const char* b) +{ + TiXmlString tmp; + TiXmlString::size_type b_len = static_cast( strlen(b) ); + tmp.reserve(a.length() + b_len); + tmp += a; + tmp.append(b, b_len); + return tmp; +} + +TiXmlString operator + (const char* a, const TiXmlString & b) +{ + TiXmlString tmp; + TiXmlString::size_type a_len = static_cast( strlen(a) ); + tmp.reserve(a_len + b.length()); + tmp.append(a, a_len); + tmp += b; + return tmp; +} + + +#endif // TIXML_USE_STL diff --git a/thirdparty/tinyxml/src/tinyxml.cpp b/thirdparty/tinyxml/src/tinyxml.cpp new file mode 100644 index 0000000..5e24724 --- /dev/null +++ b/thirdparty/tinyxml/src/tinyxml.cpp @@ -0,0 +1,1886 @@ +/* +www.sourceforge.net/projects/tinyxml +Original code by Lee Thomason (www.grinninglizard.com) + +This software is provided 'as-is', without any express or implied +warranty. In no event will the authors be held liable for any +damages arising from the use of this software. + +Permission is granted to anyone to use this software for any +purpose, including commercial applications, and to alter it and +redistribute it freely, subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must +not claim that you wrote the original software. If you use this +software in a product, an acknowledgment in the product documentation +would be appreciated but is not required. + +2. Altered source versions must be plainly marked as such, and +must not be misrepresented as being the original software. + +3. This notice may not be removed or altered from any source +distribution. +*/ + +#include + +#ifdef TIXML_USE_STL +#include +#include +#endif + +#include "tinyxml/include/tinyxml.h" + +FILE* TiXmlFOpen( const char* filename, const char* mode ); + +bool TiXmlBase::condenseWhiteSpace = true; + +// Microsoft compiler security +FILE* TiXmlFOpen( const char* filename, const char* mode ) +{ + #if defined(_MSC_VER) && (_MSC_VER >= 1400 ) + FILE* fp = 0; + errno_t err = fopen_s( &fp, filename, mode ); + if ( !err && fp ) + return fp; + return 0; + #else + return fopen( filename, mode ); + #endif +} + +void TiXmlBase::EncodeString( const TIXML_STRING& str, TIXML_STRING* outString ) +{ + int i=0; + + while( i<(int)str.length() ) + { + unsigned char c = (unsigned char) str[i]; + + if ( c == '&' + && i < ( (int)str.length() - 2 ) + && str[i+1] == '#' + && str[i+2] == 'x' ) + { + // Hexadecimal character reference. + // Pass through unchanged. + // © -- copyright symbol, for example. + // + // The -1 is a bug fix from Rob Laveaux. It keeps + // an overflow from happening if there is no ';'. + // There are actually 2 ways to exit this loop - + // while fails (error case) and break (semicolon found). + // However, there is no mechanism (currently) for + // this function to return an error. + while ( i<(int)str.length()-1 ) + { + outString->append( str.c_str() + i, 1 ); + ++i; + if ( str[i] == ';' ) + break; + } + } + else if ( c == '&' ) + { + outString->append( entity[0].str, entity[0].strLength ); + ++i; + } + else if ( c == '<' ) + { + outString->append( entity[1].str, entity[1].strLength ); + ++i; + } + else if ( c == '>' ) + { + outString->append( entity[2].str, entity[2].strLength ); + ++i; + } + else if ( c == '\"' ) + { + outString->append( entity[3].str, entity[3].strLength ); + ++i; + } + else if ( c == '\'' ) + { + outString->append( entity[4].str, entity[4].strLength ); + ++i; + } + else if ( c < 32 ) + { + // Easy pass at non-alpha/numeric/symbol + // Below 32 is symbolic. + char buf[ 32 ]; + + #if defined(TIXML_SNPRINTF) + TIXML_SNPRINTF( buf, sizeof(buf), "&#x%02X;", (unsigned) ( c & 0xff ) ); + #else + sprintf( buf, "&#x%02X;", (unsigned) ( c & 0xff ) ); + #endif + + //*ME: warning C4267: convert 'size_t' to 'int' + //*ME: Int-Cast to make compiler happy ... + outString->append( buf, (int)strlen( buf ) ); + ++i; + } + else + { + //char realc = (char) c; + //outString->append( &realc, 1 ); + *outString += (char) c; // somewhat more efficient function call. + ++i; + } + } +} + + +TiXmlNode::TiXmlNode( NodeType _type ) : TiXmlBase() +{ + parent = 0; + type = _type; + firstChild = 0; + lastChild = 0; + prev = 0; + next = 0; +} + + +TiXmlNode::~TiXmlNode() +{ + TiXmlNode* node = firstChild; + TiXmlNode* temp = 0; + + while ( node ) + { + temp = node; + node = node->next; + delete temp; + } +} + + +void TiXmlNode::CopyTo( TiXmlNode* target ) const +{ + target->SetValue (value.c_str() ); + target->userData = userData; + target->location = location; +} + + +void TiXmlNode::Clear() +{ + TiXmlNode* node = firstChild; + TiXmlNode* temp = 0; + + while ( node ) + { + temp = node; + node = node->next; + delete temp; + } + + firstChild = 0; + lastChild = 0; +} + + +TiXmlNode* TiXmlNode::LinkEndChild( TiXmlNode* node ) +{ + assert( node->parent == 0 || node->parent == this ); + assert( node->GetDocument() == 0 || node->GetDocument() == this->GetDocument() ); + + if ( node->Type() == TiXmlNode::TINYXML_DOCUMENT ) + { + delete node; + if ( GetDocument() ) + GetDocument()->SetError( TIXML_ERROR_DOCUMENT_TOP_ONLY, 0, 0, TIXML_ENCODING_UNKNOWN ); + return 0; + } + + node->parent = this; + + node->prev = lastChild; + node->next = 0; + + if ( lastChild ) + lastChild->next = node; + else + firstChild = node; // it was an empty list. + + lastChild = node; + return node; +} + + +TiXmlNode* TiXmlNode::InsertEndChild( const TiXmlNode& addThis ) +{ + if ( addThis.Type() == TiXmlNode::TINYXML_DOCUMENT ) + { + if ( GetDocument() ) + GetDocument()->SetError( TIXML_ERROR_DOCUMENT_TOP_ONLY, 0, 0, TIXML_ENCODING_UNKNOWN ); + return 0; + } + TiXmlNode* node = addThis.Clone(); + if ( !node ) + return 0; + + return LinkEndChild( node ); +} + + +TiXmlNode* TiXmlNode::InsertBeforeChild( TiXmlNode* beforeThis, const TiXmlNode& addThis ) +{ + if ( !beforeThis || beforeThis->parent != this ) { + return 0; + } + if ( addThis.Type() == TiXmlNode::TINYXML_DOCUMENT ) + { + if ( GetDocument() ) + GetDocument()->SetError( TIXML_ERROR_DOCUMENT_TOP_ONLY, 0, 0, TIXML_ENCODING_UNKNOWN ); + return 0; + } + + TiXmlNode* node = addThis.Clone(); + if ( !node ) + return 0; + node->parent = this; + + node->next = beforeThis; + node->prev = beforeThis->prev; + if ( beforeThis->prev ) + { + beforeThis->prev->next = node; + } + else + { + assert( firstChild == beforeThis ); + firstChild = node; + } + beforeThis->prev = node; + return node; +} + + +TiXmlNode* TiXmlNode::InsertAfterChild( TiXmlNode* afterThis, const TiXmlNode& addThis ) +{ + if ( !afterThis || afterThis->parent != this ) { + return 0; + } + if ( addThis.Type() == TiXmlNode::TINYXML_DOCUMENT ) + { + if ( GetDocument() ) + GetDocument()->SetError( TIXML_ERROR_DOCUMENT_TOP_ONLY, 0, 0, TIXML_ENCODING_UNKNOWN ); + return 0; + } + + TiXmlNode* node = addThis.Clone(); + if ( !node ) + return 0; + node->parent = this; + + node->prev = afterThis; + node->next = afterThis->next; + if ( afterThis->next ) + { + afterThis->next->prev = node; + } + else + { + assert( lastChild == afterThis ); + lastChild = node; + } + afterThis->next = node; + return node; +} + + +TiXmlNode* TiXmlNode::ReplaceChild( TiXmlNode* replaceThis, const TiXmlNode& withThis ) +{ + if ( !replaceThis ) + return 0; + + if ( replaceThis->parent != this ) + return 0; + + if ( withThis.ToDocument() ) { + // A document can never be a child. Thanks to Noam. + TiXmlDocument* document = GetDocument(); + if ( document ) + document->SetError( TIXML_ERROR_DOCUMENT_TOP_ONLY, 0, 0, TIXML_ENCODING_UNKNOWN ); + return 0; + } + + TiXmlNode* node = withThis.Clone(); + if ( !node ) + return 0; + + node->next = replaceThis->next; + node->prev = replaceThis->prev; + + if ( replaceThis->next ) + replaceThis->next->prev = node; + else + lastChild = node; + + if ( replaceThis->prev ) + replaceThis->prev->next = node; + else + firstChild = node; + + delete replaceThis; + node->parent = this; + return node; +} + + +bool TiXmlNode::RemoveChild( TiXmlNode* removeThis ) +{ + if ( !removeThis ) { + return false; + } + + if ( removeThis->parent != this ) + { + assert( 0 ); + return false; + } + + if ( removeThis->next ) + removeThis->next->prev = removeThis->prev; + else + lastChild = removeThis->prev; + + if ( removeThis->prev ) + removeThis->prev->next = removeThis->next; + else + firstChild = removeThis->next; + + delete removeThis; + return true; +} + +const TiXmlNode* TiXmlNode::FirstChild( const char * _value ) const +{ + const TiXmlNode* node; + for ( node = firstChild; node; node = node->next ) + { + if ( strcmp( node->Value(), _value ) == 0 ) + return node; + } + return 0; +} + + +const TiXmlNode* TiXmlNode::LastChild( const char * _value ) const +{ + const TiXmlNode* node; + for ( node = lastChild; node; node = node->prev ) + { + if ( strcmp( node->Value(), _value ) == 0 ) + return node; + } + return 0; +} + + +const TiXmlNode* TiXmlNode::IterateChildren( const TiXmlNode* previous ) const +{ + if ( !previous ) + { + return FirstChild(); + } + else + { + assert( previous->parent == this ); + return previous->NextSibling(); + } +} + + +const TiXmlNode* TiXmlNode::IterateChildren( const char * val, const TiXmlNode* previous ) const +{ + if ( !previous ) + { + return FirstChild( val ); + } + else + { + assert( previous->parent == this ); + return previous->NextSibling( val ); + } +} + + +const TiXmlNode* TiXmlNode::NextSibling( const char * _value ) const +{ + const TiXmlNode* node; + for ( node = next; node; node = node->next ) + { + if ( strcmp( node->Value(), _value ) == 0 ) + return node; + } + return 0; +} + + +const TiXmlNode* TiXmlNode::PreviousSibling( const char * _value ) const +{ + const TiXmlNode* node; + for ( node = prev; node; node = node->prev ) + { + if ( strcmp( node->Value(), _value ) == 0 ) + return node; + } + return 0; +} + + +void TiXmlElement::RemoveAttribute( const char * name ) +{ + #ifdef TIXML_USE_STL + TIXML_STRING str( name ); + TiXmlAttribute* node = attributeSet.Find( str ); + #else + TiXmlAttribute* node = attributeSet.Find( name ); + #endif + if ( node ) + { + attributeSet.Remove( node ); + delete node; + } +} + +const TiXmlElement* TiXmlNode::FirstChildElement() const +{ + const TiXmlNode* node; + + for ( node = FirstChild(); + node; + node = node->NextSibling() ) + { + if ( node->ToElement() ) + return node->ToElement(); + } + return 0; +} + + +const TiXmlElement* TiXmlNode::FirstChildElement( const char * _value ) const +{ + const TiXmlNode* node; + + for ( node = FirstChild( _value ); + node; + node = node->NextSibling( _value ) ) + { + if ( node->ToElement() ) + return node->ToElement(); + } + return 0; +} + + +const TiXmlElement* TiXmlNode::NextSiblingElement() const +{ + const TiXmlNode* node; + + for ( node = NextSibling(); + node; + node = node->NextSibling() ) + { + if ( node->ToElement() ) + return node->ToElement(); + } + return 0; +} + + +const TiXmlElement* TiXmlNode::NextSiblingElement( const char * _value ) const +{ + const TiXmlNode* node; + + for ( node = NextSibling( _value ); + node; + node = node->NextSibling( _value ) ) + { + if ( node->ToElement() ) + return node->ToElement(); + } + return 0; +} + + +const TiXmlDocument* TiXmlNode::GetDocument() const +{ + const TiXmlNode* node; + + for( node = this; node; node = node->parent ) + { + if ( node->ToDocument() ) + return node->ToDocument(); + } + return 0; +} + + +TiXmlElement::TiXmlElement (const char * _value) + : TiXmlNode( TiXmlNode::TINYXML_ELEMENT ) +{ + firstChild = lastChild = 0; + value = _value; +} + + +#ifdef TIXML_USE_STL +TiXmlElement::TiXmlElement( const std::string& _value ) + : TiXmlNode( TiXmlNode::TINYXML_ELEMENT ) +{ + firstChild = lastChild = 0; + value = _value; +} +#endif + + +TiXmlElement::TiXmlElement( const TiXmlElement& copy) + : TiXmlNode( TiXmlNode::TINYXML_ELEMENT ) +{ + firstChild = lastChild = 0; + copy.CopyTo( this ); +} + + +TiXmlElement& TiXmlElement::operator=( const TiXmlElement& base ) +{ + ClearThis(); + base.CopyTo( this ); + return *this; +} + + +TiXmlElement::~TiXmlElement() +{ + ClearThis(); +} + + +void TiXmlElement::ClearThis() +{ + Clear(); + while( attributeSet.First() ) + { + TiXmlAttribute* node = attributeSet.First(); + attributeSet.Remove( node ); + delete node; + } +} + + +const char* TiXmlElement::Attribute( const char* name ) const +{ + const TiXmlAttribute* node = attributeSet.Find( name ); + if ( node ) + return node->Value(); + return 0; +} + + +#ifdef TIXML_USE_STL +const std::string* TiXmlElement::Attribute( const std::string& name ) const +{ + const TiXmlAttribute* attrib = attributeSet.Find( name ); + if ( attrib ) + return &attrib->ValueStr(); + return 0; +} +#endif + + +const char* TiXmlElement::Attribute( const char* name, int* i ) const +{ + const TiXmlAttribute* attrib = attributeSet.Find( name ); + const char* result = 0; + + if ( attrib ) { + result = attrib->Value(); + if ( i ) { + attrib->QueryIntValue( i ); + } + } + return result; +} + + +#ifdef TIXML_USE_STL +const std::string* TiXmlElement::Attribute( const std::string& name, int* i ) const +{ + const TiXmlAttribute* attrib = attributeSet.Find( name ); + const std::string* result = 0; + + if ( attrib ) { + result = &attrib->ValueStr(); + if ( i ) { + attrib->QueryIntValue( i ); + } + } + return result; +} +#endif + + +const char* TiXmlElement::Attribute( const char* name, double* d ) const +{ + const TiXmlAttribute* attrib = attributeSet.Find( name ); + const char* result = 0; + + if ( attrib ) { + result = attrib->Value(); + if ( d ) { + attrib->QueryDoubleValue( d ); + } + } + return result; +} + + +#ifdef TIXML_USE_STL +const std::string* TiXmlElement::Attribute( const std::string& name, double* d ) const +{ + const TiXmlAttribute* attrib = attributeSet.Find( name ); + const std::string* result = 0; + + if ( attrib ) { + result = &attrib->ValueStr(); + if ( d ) { + attrib->QueryDoubleValue( d ); + } + } + return result; +} +#endif + + +int TiXmlElement::QueryIntAttribute( const char* name, int* ival ) const +{ + const TiXmlAttribute* attrib = attributeSet.Find( name ); + if ( !attrib ) + return TIXML_NO_ATTRIBUTE; + return attrib->QueryIntValue( ival ); +} + + +int TiXmlElement::QueryUnsignedAttribute( const char* name, unsigned* value ) const +{ + const TiXmlAttribute* node = attributeSet.Find( name ); + if ( !node ) + return TIXML_NO_ATTRIBUTE; + + int ival = 0; + int result = node->QueryIntValue( &ival ); + *value = (unsigned)ival; + return result; +} + + +int TiXmlElement::QueryBoolAttribute( const char* name, bool* bval ) const +{ + const TiXmlAttribute* node = attributeSet.Find( name ); + if ( !node ) + return TIXML_NO_ATTRIBUTE; + + int result = TIXML_WRONG_TYPE; + if ( StringEqual( node->Value(), "true", true, TIXML_ENCODING_UNKNOWN ) + || StringEqual( node->Value(), "yes", true, TIXML_ENCODING_UNKNOWN ) + || StringEqual( node->Value(), "1", true, TIXML_ENCODING_UNKNOWN ) ) + { + *bval = true; + result = TIXML_SUCCESS; + } + else if ( StringEqual( node->Value(), "false", true, TIXML_ENCODING_UNKNOWN ) + || StringEqual( node->Value(), "no", true, TIXML_ENCODING_UNKNOWN ) + || StringEqual( node->Value(), "0", true, TIXML_ENCODING_UNKNOWN ) ) + { + *bval = false; + result = TIXML_SUCCESS; + } + return result; +} + + + +#ifdef TIXML_USE_STL +int TiXmlElement::QueryIntAttribute( const std::string& name, int* ival ) const +{ + const TiXmlAttribute* attrib = attributeSet.Find( name ); + if ( !attrib ) + return TIXML_NO_ATTRIBUTE; + return attrib->QueryIntValue( ival ); +} +#endif + + +int TiXmlElement::QueryDoubleAttribute( const char* name, double* dval ) const +{ + const TiXmlAttribute* attrib = attributeSet.Find( name ); + if ( !attrib ) + return TIXML_NO_ATTRIBUTE; + return attrib->QueryDoubleValue( dval ); +} + + +#ifdef TIXML_USE_STL +int TiXmlElement::QueryDoubleAttribute( const std::string& name, double* dval ) const +{ + const TiXmlAttribute* attrib = attributeSet.Find( name ); + if ( !attrib ) + return TIXML_NO_ATTRIBUTE; + return attrib->QueryDoubleValue( dval ); +} +#endif + + +void TiXmlElement::SetAttribute( const char * name, int val ) +{ + TiXmlAttribute* attrib = attributeSet.FindOrCreate( name ); + if ( attrib ) { + attrib->SetIntValue( val ); + } +} + + +#ifdef TIXML_USE_STL +void TiXmlElement::SetAttribute( const std::string& name, int val ) +{ + TiXmlAttribute* attrib = attributeSet.FindOrCreate( name ); + if ( attrib ) { + attrib->SetIntValue( val ); + } +} +#endif + + +void TiXmlElement::SetDoubleAttribute( const char * name, double val ) +{ + TiXmlAttribute* attrib = attributeSet.FindOrCreate( name ); + if ( attrib ) { + attrib->SetDoubleValue( val ); + } +} + + +#ifdef TIXML_USE_STL +void TiXmlElement::SetDoubleAttribute( const std::string& name, double val ) +{ + TiXmlAttribute* attrib = attributeSet.FindOrCreate( name ); + if ( attrib ) { + attrib->SetDoubleValue( val ); + } +} +#endif + + +void TiXmlElement::SetAttribute( const char * cname, const char * cvalue ) +{ + TiXmlAttribute* attrib = attributeSet.FindOrCreate( cname ); + if ( attrib ) { + attrib->SetValue( cvalue ); + } +} + + +#ifdef TIXML_USE_STL +void TiXmlElement::SetAttribute( const std::string& _name, const std::string& _value ) +{ + TiXmlAttribute* attrib = attributeSet.FindOrCreate( _name ); + if ( attrib ) { + attrib->SetValue( _value ); + } +} +#endif + + +void TiXmlElement::Print( FILE* cfile, int depth ) const +{ + int i; + assert( cfile ); + for ( i=0; iNext() ) + { + fprintf( cfile, " " ); + attrib->Print( cfile, depth ); + } + + // There are 3 different formatting approaches: + // 1) An element without children is printed as a node + // 2) An element with only a text child is printed as text + // 3) An element with children is printed on multiple lines. + TiXmlNode* node; + if ( !firstChild ) + { + fprintf( cfile, " />" ); + } + else if ( firstChild == lastChild && firstChild->ToText() ) + { + fprintf( cfile, ">" ); + firstChild->Print( cfile, depth + 1 ); + fprintf( cfile, "", value.c_str() ); + } + else + { + fprintf( cfile, ">" ); + + for ( node = firstChild; node; node=node->NextSibling() ) + { + if ( !node->ToText() ) + { + fprintf( cfile, "\n" ); + } + node->Print( cfile, depth+1 ); + } + fprintf( cfile, "\n" ); + for( i=0; i", value.c_str() ); + } +} + + +void TiXmlElement::CopyTo( TiXmlElement* target ) const +{ + // superclass: + TiXmlNode::CopyTo( target ); + + // Element class: + // Clone the attributes, then clone the children. + const TiXmlAttribute* attribute = 0; + for( attribute = attributeSet.First(); + attribute; + attribute = attribute->Next() ) + { + target->SetAttribute( attribute->Name(), attribute->Value() ); + } + + TiXmlNode* node = 0; + for ( node = firstChild; node; node = node->NextSibling() ) + { + target->LinkEndChild( node->Clone() ); + } +} + +bool TiXmlElement::Accept( TiXmlVisitor* visitor ) const +{ + if ( visitor->VisitEnter( *this, attributeSet.First() ) ) + { + for ( const TiXmlNode* node=FirstChild(); node; node=node->NextSibling() ) + { + if ( !node->Accept( visitor ) ) + break; + } + } + return visitor->VisitExit( *this ); +} + + +TiXmlNode* TiXmlElement::Clone() const +{ + TiXmlElement* clone = new TiXmlElement( Value() ); + if ( !clone ) + return 0; + + CopyTo( clone ); + return clone; +} + + +const char* TiXmlElement::GetText() const +{ + const TiXmlNode* child = this->FirstChild(); + if ( child ) { + const TiXmlText* childText = child->ToText(); + if ( childText ) { + return childText->Value(); + } + } + return 0; +} + + +TiXmlDocument::TiXmlDocument() : TiXmlNode( TiXmlNode::TINYXML_DOCUMENT ) +{ + tabsize = 4; + useMicrosoftBOM = false; + ClearError(); +} + +TiXmlDocument::TiXmlDocument( const char * documentName ) : TiXmlNode( TiXmlNode::TINYXML_DOCUMENT ) +{ + tabsize = 4; + useMicrosoftBOM = false; + value = documentName; + ClearError(); +} + + +#ifdef TIXML_USE_STL +TiXmlDocument::TiXmlDocument( const std::string& documentName ) : TiXmlNode( TiXmlNode::TINYXML_DOCUMENT ) +{ + tabsize = 4; + useMicrosoftBOM = false; + value = documentName; + ClearError(); +} +#endif + + +TiXmlDocument::TiXmlDocument( const TiXmlDocument& copy ) : TiXmlNode( TiXmlNode::TINYXML_DOCUMENT ) +{ + copy.CopyTo( this ); +} + + +TiXmlDocument& TiXmlDocument::operator=( const TiXmlDocument& copy ) +{ + Clear(); + copy.CopyTo( this ); + return *this; +} + + +bool TiXmlDocument::LoadFile( TiXmlEncoding encoding ) +{ + return LoadFile( Value(), encoding ); +} + + +bool TiXmlDocument::SaveFile() const +{ + return SaveFile( Value() ); +} + +bool TiXmlDocument::LoadFile( const char* _filename, TiXmlEncoding encoding ) +{ + TIXML_STRING filename( _filename ); + value = filename; + + // reading in binary mode so that tinyxml can normalize the EOL + FILE* file = TiXmlFOpen( value.c_str (), "rb" ); + + if ( file ) + { + bool result = LoadFile( file, encoding ); + fclose( file ); + return result; + } + else + { + SetError( TIXML_ERROR_OPENING_FILE, 0, 0, TIXML_ENCODING_UNKNOWN ); + return false; + } +} + +bool TiXmlDocument::LoadFile( FILE* file, TiXmlEncoding encoding ) +{ + if ( !file ) + { + SetError( TIXML_ERROR_OPENING_FILE, 0, 0, TIXML_ENCODING_UNKNOWN ); + return false; + } + + // Delete the existing data: + Clear(); + location.Clear(); + + // Get the file size, so we can pre-allocate the string. HUGE speed impact. + long length = 0; + fseek( file, 0, SEEK_END ); + length = ftell( file ); + fseek( file, 0, SEEK_SET ); + + // Strange case, but good to handle up front. + if ( length <= 0 ) + { + SetError( TIXML_ERROR_DOCUMENT_EMPTY, 0, 0, TIXML_ENCODING_UNKNOWN ); + return false; + } + + // Subtle bug here. TinyXml did use fgets. But from the XML spec: + // 2.11 End-of-Line Handling + // + // + // ...the XML processor MUST behave as if it normalized all line breaks in external + // parsed entities (including the document entity) on input, before parsing, by translating + // both the two-character sequence #xD #xA and any #xD that is not followed by #xA to + // a single #xA character. + // + // + // It is not clear fgets does that, and certainly isn't clear it works cross platform. + // Generally, you expect fgets to translate from the convention of the OS to the c/unix + // convention, and not work generally. + + /* + while( fgets( buf, sizeof(buf), file ) ) + { + data += buf; + } + */ + + char* buf = new char[ length+1 ]; + buf[0] = 0; + + if ( fread( buf, length, 1, file ) != 1 ) { + delete [] buf; + SetError( TIXML_ERROR_OPENING_FILE, 0, 0, TIXML_ENCODING_UNKNOWN ); + return false; + } + + // Process the buffer in place to normalize new lines. (See comment above.) + // Copies from the 'p' to 'q' pointer, where p can advance faster if + // a newline-carriage return is hit. + // + // Wikipedia: + // Systems based on ASCII or a compatible character set use either LF (Line feed, '\n', 0x0A, 10 in decimal) or + // CR (Carriage return, '\r', 0x0D, 13 in decimal) individually, or CR followed by LF (CR+LF, 0x0D 0x0A)... + // * LF: Multics, Unix and Unix-like systems (GNU/Linux, AIX, Xenix, Mac OS X, FreeBSD, etc.), BeOS, Amiga, RISC OS, and others + // * CR+LF: DEC RT-11 and most other early non-Unix, non-IBM OSes, CP/M, MP/M, DOS, OS/2, Microsoft Windows, Symbian OS + // * CR: Commodore 8-bit machines, Apple II family, Mac OS up to version 9 and OS-9 + + const char* p = buf; // the read head + char* q = buf; // the write head + const char CR = 0x0d; + const char LF = 0x0a; + + buf[length] = 0; + while( *p ) { + assert( p < (buf+length) ); + assert( q <= (buf+length) ); + assert( q <= p ); + + if ( *p == CR ) { + *q++ = LF; + p++; + if ( *p == LF ) { // check for CR+LF (and skip LF) + p++; + } + } + else { + *q++ = *p++; + } + } + assert( q <= (buf+length) ); + *q = 0; + + Parse( buf, 0, encoding ); + + delete [] buf; + return !Error(); +} + + +bool TiXmlDocument::SaveFile( const char * filename ) const +{ + // The old c stuff lives on... + FILE* fp = TiXmlFOpen( filename, "w" ); + if ( fp ) + { + bool result = SaveFile( fp ); + fclose( fp ); + return result; + } + return false; +} + + +bool TiXmlDocument::SaveFile( FILE* fp ) const +{ + if ( useMicrosoftBOM ) + { + const unsigned char TIXML_UTF_LEAD_0 = 0xefU; + const unsigned char TIXML_UTF_LEAD_1 = 0xbbU; + const unsigned char TIXML_UTF_LEAD_2 = 0xbfU; + + fputc( TIXML_UTF_LEAD_0, fp ); + fputc( TIXML_UTF_LEAD_1, fp ); + fputc( TIXML_UTF_LEAD_2, fp ); + } + Print( fp, 0 ); + return (ferror(fp) == 0); +} + + +void TiXmlDocument::CopyTo( TiXmlDocument* target ) const +{ + TiXmlNode::CopyTo( target ); + + target->error = error; + target->errorId = errorId; + target->errorDesc = errorDesc; + target->tabsize = tabsize; + target->errorLocation = errorLocation; + target->useMicrosoftBOM = useMicrosoftBOM; + + TiXmlNode* node = 0; + for ( node = firstChild; node; node = node->NextSibling() ) + { + target->LinkEndChild( node->Clone() ); + } +} + + +TiXmlNode* TiXmlDocument::Clone() const +{ + TiXmlDocument* clone = new TiXmlDocument(); + if ( !clone ) + return 0; + + CopyTo( clone ); + return clone; +} + + +void TiXmlDocument::Print( FILE* cfile, int depth ) const +{ + assert( cfile ); + for ( const TiXmlNode* node=FirstChild(); node; node=node->NextSibling() ) + { + node->Print( cfile, depth ); + fprintf( cfile, "\n" ); + } +} + + +bool TiXmlDocument::Accept( TiXmlVisitor* visitor ) const +{ + if ( visitor->VisitEnter( *this ) ) + { + for ( const TiXmlNode* node=FirstChild(); node; node=node->NextSibling() ) + { + if ( !node->Accept( visitor ) ) + break; + } + } + return visitor->VisitExit( *this ); +} + + +const TiXmlAttribute* TiXmlAttribute::Next() const +{ + // We are using knowledge of the sentinel. The sentinel + // have a value or name. + if ( next->value.empty() && next->name.empty() ) + return 0; + return next; +} + +/* +TiXmlAttribute* TiXmlAttribute::Next() +{ + // We are using knowledge of the sentinel. The sentinel + // have a value or name. + if ( next->value.empty() && next->name.empty() ) + return 0; + return next; +} +*/ + +const TiXmlAttribute* TiXmlAttribute::Previous() const +{ + // We are using knowledge of the sentinel. The sentinel + // have a value or name. + if ( prev->value.empty() && prev->name.empty() ) + return 0; + return prev; +} + +/* +TiXmlAttribute* TiXmlAttribute::Previous() +{ + // We are using knowledge of the sentinel. The sentinel + // have a value or name. + if ( prev->value.empty() && prev->name.empty() ) + return 0; + return prev; +} +*/ + +void TiXmlAttribute::Print( FILE* cfile, int /*depth*/, TIXML_STRING* str ) const +{ + TIXML_STRING n, v; + + EncodeString( name, &n ); + EncodeString( value, &v ); + + if (value.find ('\"') == TIXML_STRING::npos) { + if ( cfile ) { + fprintf (cfile, "%s=\"%s\"", n.c_str(), v.c_str() ); + } + if ( str ) { + (*str) += n; (*str) += "=\""; (*str) += v; (*str) += "\""; + } + } + else { + if ( cfile ) { + fprintf (cfile, "%s='%s'", n.c_str(), v.c_str() ); + } + if ( str ) { + (*str) += n; (*str) += "='"; (*str) += v; (*str) += "'"; + } + } +} + + +int TiXmlAttribute::QueryIntValue( int* ival ) const +{ + if ( TIXML_SSCANF( value.c_str(), "%d", ival ) == 1 ) + return TIXML_SUCCESS; + return TIXML_WRONG_TYPE; +} + +int TiXmlAttribute::QueryDoubleValue( double* dval ) const +{ + if ( TIXML_SSCANF( value.c_str(), "%lf", dval ) == 1 ) + return TIXML_SUCCESS; + return TIXML_WRONG_TYPE; +} + +void TiXmlAttribute::SetIntValue( int _value ) +{ + char buf [64]; + #if defined(TIXML_SNPRINTF) + TIXML_SNPRINTF(buf, sizeof(buf), "%d", _value); + #else + sprintf (buf, "%d", _value); + #endif + SetValue (buf); +} + +void TiXmlAttribute::SetDoubleValue( double _value ) +{ + char buf [256]; + #if defined(TIXML_SNPRINTF) + TIXML_SNPRINTF( buf, sizeof(buf), "%g", _value); + #else + sprintf (buf, "%g", _value); + #endif + SetValue (buf); +} + +int TiXmlAttribute::IntValue() const +{ + return atoi (value.c_str ()); +} + +double TiXmlAttribute::DoubleValue() const +{ + return atof (value.c_str ()); +} + + +TiXmlComment::TiXmlComment( const TiXmlComment& copy ) : TiXmlNode( TiXmlNode::TINYXML_COMMENT ) +{ + copy.CopyTo( this ); +} + + +TiXmlComment& TiXmlComment::operator=( const TiXmlComment& base ) +{ + Clear(); + base.CopyTo( this ); + return *this; +} + + +void TiXmlComment::Print( FILE* cfile, int depth ) const +{ + assert( cfile ); + for ( int i=0; i", value.c_str() ); +} + + +void TiXmlComment::CopyTo( TiXmlComment* target ) const +{ + TiXmlNode::CopyTo( target ); +} + + +bool TiXmlComment::Accept( TiXmlVisitor* visitor ) const +{ + return visitor->Visit( *this ); +} + + +TiXmlNode* TiXmlComment::Clone() const +{ + TiXmlComment* clone = new TiXmlComment(); + + if ( !clone ) + return 0; + + CopyTo( clone ); + return clone; +} + + +void TiXmlText::Print( FILE* cfile, int depth ) const +{ + assert( cfile ); + if ( cdata ) + { + int i; + fprintf( cfile, "\n" ); + for ( i=0; i\n", value.c_str() ); // unformatted output + } + else + { + TIXML_STRING buffer; + EncodeString( value, &buffer ); + fprintf( cfile, "%s", buffer.c_str() ); + } +} + + +void TiXmlText::CopyTo( TiXmlText* target ) const +{ + TiXmlNode::CopyTo( target ); + target->cdata = cdata; +} + + +bool TiXmlText::Accept( TiXmlVisitor* visitor ) const +{ + return visitor->Visit( *this ); +} + + +TiXmlNode* TiXmlText::Clone() const +{ + TiXmlText* clone = 0; + clone = new TiXmlText( "" ); + + if ( !clone ) + return 0; + + CopyTo( clone ); + return clone; +} + + +TiXmlDeclaration::TiXmlDeclaration( const char * _version, + const char * _encoding, + const char * _standalone ) + : TiXmlNode( TiXmlNode::TINYXML_DECLARATION ) +{ + version = _version; + encoding = _encoding; + standalone = _standalone; +} + + +#ifdef TIXML_USE_STL +TiXmlDeclaration::TiXmlDeclaration( const std::string& _version, + const std::string& _encoding, + const std::string& _standalone ) + : TiXmlNode( TiXmlNode::TINYXML_DECLARATION ) +{ + version = _version; + encoding = _encoding; + standalone = _standalone; +} +#endif + + +TiXmlDeclaration::TiXmlDeclaration( const TiXmlDeclaration& copy ) + : TiXmlNode( TiXmlNode::TINYXML_DECLARATION ) +{ + copy.CopyTo( this ); +} + + +TiXmlDeclaration& TiXmlDeclaration::operator=( const TiXmlDeclaration& copy ) +{ + Clear(); + copy.CopyTo( this ); + return *this; +} + + +void TiXmlDeclaration::Print( FILE* cfile, int /*depth*/, TIXML_STRING* str ) const +{ + if ( cfile ) fprintf( cfile, "" ); + if ( str ) (*str) += "?>"; +} + + +void TiXmlDeclaration::CopyTo( TiXmlDeclaration* target ) const +{ + TiXmlNode::CopyTo( target ); + + target->version = version; + target->encoding = encoding; + target->standalone = standalone; +} + + +bool TiXmlDeclaration::Accept( TiXmlVisitor* visitor ) const +{ + return visitor->Visit( *this ); +} + + +TiXmlNode* TiXmlDeclaration::Clone() const +{ + TiXmlDeclaration* clone = new TiXmlDeclaration(); + + if ( !clone ) + return 0; + + CopyTo( clone ); + return clone; +} + + +void TiXmlUnknown::Print( FILE* cfile, int depth ) const +{ + for ( int i=0; i", value.c_str() ); +} + + +void TiXmlUnknown::CopyTo( TiXmlUnknown* target ) const +{ + TiXmlNode::CopyTo( target ); +} + + +bool TiXmlUnknown::Accept( TiXmlVisitor* visitor ) const +{ + return visitor->Visit( *this ); +} + + +TiXmlNode* TiXmlUnknown::Clone() const +{ + TiXmlUnknown* clone = new TiXmlUnknown(); + + if ( !clone ) + return 0; + + CopyTo( clone ); + return clone; +} + + +TiXmlAttributeSet::TiXmlAttributeSet() +{ + sentinel.next = &sentinel; + sentinel.prev = &sentinel; +} + + +TiXmlAttributeSet::~TiXmlAttributeSet() +{ + assert( sentinel.next == &sentinel ); + assert( sentinel.prev == &sentinel ); +} + + +void TiXmlAttributeSet::Add( TiXmlAttribute* addMe ) +{ + #ifdef TIXML_USE_STL + assert( !Find( TIXML_STRING( addMe->Name() ) ) ); // Shouldn't be multiply adding to the set. + #else + assert( !Find( addMe->Name() ) ); // Shouldn't be multiply adding to the set. + #endif + + addMe->next = &sentinel; + addMe->prev = sentinel.prev; + + sentinel.prev->next = addMe; + sentinel.prev = addMe; +} + +void TiXmlAttributeSet::Remove( TiXmlAttribute* removeMe ) +{ + TiXmlAttribute* node; + + for( node = sentinel.next; node != &sentinel; node = node->next ) + { + if ( node == removeMe ) + { + node->prev->next = node->next; + node->next->prev = node->prev; + node->next = 0; + node->prev = 0; + return; + } + } + assert( 0 ); // we tried to remove a non-linked attribute. +} + + +#ifdef TIXML_USE_STL +TiXmlAttribute* TiXmlAttributeSet::Find( const std::string& name ) const +{ + for( TiXmlAttribute* node = sentinel.next; node != &sentinel; node = node->next ) + { + if ( node->name == name ) + return node; + } + return 0; +} + +TiXmlAttribute* TiXmlAttributeSet::FindOrCreate( const std::string& _name ) +{ + TiXmlAttribute* attrib = Find( _name ); + if ( !attrib ) { + attrib = new TiXmlAttribute(); + Add( attrib ); + attrib->SetName( _name ); + } + return attrib; +} +#endif + + +TiXmlAttribute* TiXmlAttributeSet::Find( const char* name ) const +{ + for( TiXmlAttribute* node = sentinel.next; node != &sentinel; node = node->next ) + { + if ( strcmp( node->name.c_str(), name ) == 0 ) + return node; + } + return 0; +} + + +TiXmlAttribute* TiXmlAttributeSet::FindOrCreate( const char* _name ) +{ + TiXmlAttribute* attrib = Find( _name ); + if ( !attrib ) { + attrib = new TiXmlAttribute(); + Add( attrib ); + attrib->SetName( _name ); + } + return attrib; +} + + +#ifdef TIXML_USE_STL +std::istream& operator>> (std::istream & in, TiXmlNode & base) +{ + TIXML_STRING tag; + tag.reserve( 8 * 1000 ); + base.StreamIn( &in, &tag ); + + base.Parse( tag.c_str(), 0, TIXML_DEFAULT_ENCODING ); + return in; +} +#endif + + +#ifdef TIXML_USE_STL +std::ostream& operator<< (std::ostream & out, const TiXmlNode & base) +{ + TiXmlPrinter printer; + printer.SetStreamPrinting(); + base.Accept( &printer ); + out << printer.Str(); + + return out; +} + + +std::string& operator<< (std::string& out, const TiXmlNode& base ) +{ + TiXmlPrinter printer; + printer.SetStreamPrinting(); + base.Accept( &printer ); + out.append( printer.Str() ); + + return out; +} +#endif + + +TiXmlHandle TiXmlHandle::FirstChild() const +{ + if ( node ) + { + TiXmlNode* child = node->FirstChild(); + if ( child ) + return TiXmlHandle( child ); + } + return TiXmlHandle( 0 ); +} + + +TiXmlHandle TiXmlHandle::FirstChild( const char * value ) const +{ + if ( node ) + { + TiXmlNode* child = node->FirstChild( value ); + if ( child ) + return TiXmlHandle( child ); + } + return TiXmlHandle( 0 ); +} + + +TiXmlHandle TiXmlHandle::FirstChildElement() const +{ + if ( node ) + { + TiXmlElement* child = node->FirstChildElement(); + if ( child ) + return TiXmlHandle( child ); + } + return TiXmlHandle( 0 ); +} + + +TiXmlHandle TiXmlHandle::FirstChildElement( const char * value ) const +{ + if ( node ) + { + TiXmlElement* child = node->FirstChildElement( value ); + if ( child ) + return TiXmlHandle( child ); + } + return TiXmlHandle( 0 ); +} + + +TiXmlHandle TiXmlHandle::Child( int count ) const +{ + if ( node ) + { + int i; + TiXmlNode* child = node->FirstChild(); + for ( i=0; + child && iNextSibling(), ++i ) + { + // nothing + } + if ( child ) + return TiXmlHandle( child ); + } + return TiXmlHandle( 0 ); +} + + +TiXmlHandle TiXmlHandle::Child( const char* value, int count ) const +{ + if ( node ) + { + int i; + TiXmlNode* child = node->FirstChild( value ); + for ( i=0; + child && iNextSibling( value ), ++i ) + { + // nothing + } + if ( child ) + return TiXmlHandle( child ); + } + return TiXmlHandle( 0 ); +} + + +TiXmlHandle TiXmlHandle::ChildElement( int count ) const +{ + if ( node ) + { + int i; + TiXmlElement* child = node->FirstChildElement(); + for ( i=0; + child && iNextSiblingElement(), ++i ) + { + // nothing + } + if ( child ) + return TiXmlHandle( child ); + } + return TiXmlHandle( 0 ); +} + + +TiXmlHandle TiXmlHandle::ChildElement( const char* value, int count ) const +{ + if ( node ) + { + int i; + TiXmlElement* child = node->FirstChildElement( value ); + for ( i=0; + child && iNextSiblingElement( value ), ++i ) + { + // nothing + } + if ( child ) + return TiXmlHandle( child ); + } + return TiXmlHandle( 0 ); +} + + +bool TiXmlPrinter::VisitEnter( const TiXmlDocument& ) +{ + return true; +} + +bool TiXmlPrinter::VisitExit( const TiXmlDocument& ) +{ + return true; +} + +bool TiXmlPrinter::VisitEnter( const TiXmlElement& element, const TiXmlAttribute* firstAttribute ) +{ + DoIndent(); + buffer += "<"; + buffer += element.Value(); + + for( const TiXmlAttribute* attrib = firstAttribute; attrib; attrib = attrib->Next() ) + { + buffer += " "; + attrib->Print( 0, 0, &buffer ); + } + + if ( !element.FirstChild() ) + { + buffer += " />"; + DoLineBreak(); + } + else + { + buffer += ">"; + if ( element.FirstChild()->ToText() + && element.LastChild() == element.FirstChild() + && element.FirstChild()->ToText()->CDATA() == false ) + { + simpleTextPrint = true; + // no DoLineBreak()! + } + else + { + DoLineBreak(); + } + } + ++depth; + return true; +} + + +bool TiXmlPrinter::VisitExit( const TiXmlElement& element ) +{ + --depth; + if ( !element.FirstChild() ) + { + // nothing. + } + else + { + if ( simpleTextPrint ) + { + simpleTextPrint = false; + } + else + { + DoIndent(); + } + buffer += ""; + DoLineBreak(); + } + return true; +} + + +bool TiXmlPrinter::Visit( const TiXmlText& text ) +{ + if ( text.CDATA() ) + { + DoIndent(); + buffer += ""; + DoLineBreak(); + } + else if ( simpleTextPrint ) + { + TIXML_STRING str; + TiXmlBase::EncodeString( text.ValueTStr(), &str ); + buffer += str; + } + else + { + DoIndent(); + TIXML_STRING str; + TiXmlBase::EncodeString( text.ValueTStr(), &str ); + buffer += str; + DoLineBreak(); + } + return true; +} + + +bool TiXmlPrinter::Visit( const TiXmlDeclaration& declaration ) +{ + DoIndent(); + declaration.Print( 0, 0, &buffer ); + DoLineBreak(); + return true; +} + + +bool TiXmlPrinter::Visit( const TiXmlComment& comment ) +{ + DoIndent(); + buffer += ""; + DoLineBreak(); + return true; +} + + +bool TiXmlPrinter::Visit( const TiXmlUnknown& unknown ) +{ + DoIndent(); + buffer += "<"; + buffer += unknown.Value(); + buffer += ">"; + DoLineBreak(); + return true; +} + diff --git a/thirdparty/tinyxml/src/tinyxmlerror.cpp b/thirdparty/tinyxml/src/tinyxmlerror.cpp new file mode 100644 index 0000000..84d0410 --- /dev/null +++ b/thirdparty/tinyxml/src/tinyxmlerror.cpp @@ -0,0 +1,52 @@ +/* +www.sourceforge.net/projects/tinyxml +Original code (2.0 and earlier )copyright (c) 2000-2006 Lee Thomason (www.grinninglizard.com) + +This software is provided 'as-is', without any express or implied +warranty. In no event will the authors be held liable for any +damages arising from the use of this software. + +Permission is granted to anyone to use this software for any +purpose, including commercial applications, and to alter it and +redistribute it freely, subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must +not claim that you wrote the original software. If you use this +software in a product, an acknowledgment in the product documentation +would be appreciated but is not required. + +2. Altered source versions must be plainly marked as such, and +must not be misrepresented as being the original software. + +3. This notice may not be removed or altered from any source +distribution. +*/ + +#include "tinyxml/include/tinyxml.h" + +// The goal of the seperate error file is to make the first +// step towards localization. tinyxml (currently) only supports +// english error messages, but the could now be translated. +// +// It also cleans up the code a bit. +// + +const char* TiXmlBase::errorString[ TiXmlBase::TIXML_ERROR_STRING_COUNT ] = +{ + "No error", + "Error", + "Failed to open file", + "Error parsing Element.", + "Failed to read Element name", + "Error reading Element value.", + "Error reading Attributes.", + "Error: empty tag.", + "Error reading end tag.", + "Error parsing Unknown.", + "Error parsing Comment.", + "Error parsing Declaration.", + "Error document empty.", + "Error null (0) or unexpected EOF found in input stream.", + "Error parsing CDATA.", + "Error when TiXmlDocument added to document, because TiXmlDocument can only be at the root.", +}; diff --git a/thirdparty/tinyxml/src/tinyxmlparser.cpp b/thirdparty/tinyxml/src/tinyxmlparser.cpp new file mode 100644 index 0000000..3f82f03 --- /dev/null +++ b/thirdparty/tinyxml/src/tinyxmlparser.cpp @@ -0,0 +1,1638 @@ +/* +www.sourceforge.net/projects/tinyxml +Original code by Lee Thomason (www.grinninglizard.com) + +This software is provided 'as-is', without any express or implied +warranty. In no event will the authors be held liable for any +damages arising from the use of this software. + +Permission is granted to anyone to use this software for any +purpose, including commercial applications, and to alter it and +redistribute it freely, subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must +not claim that you wrote the original software. If you use this +software in a product, an acknowledgment in the product documentation +would be appreciated but is not required. + +2. Altered source versions must be plainly marked as such, and +must not be misrepresented as being the original software. + +3. This notice may not be removed or altered from any source +distribution. +*/ + +#include +#include + +#include "tinyxml/include/tinyxml.h" + +//#define DEBUG_PARSER +#if defined( DEBUG_PARSER ) +# if defined( DEBUG ) && defined( _MSC_VER ) +# include +# define TIXML_LOG OutputDebugString +# else +# define TIXML_LOG printf +# endif +#endif + +// Note tha "PutString" hardcodes the same list. This +// is less flexible than it appears. Changing the entries +// or order will break putstring. +TiXmlBase::Entity TiXmlBase::entity[ TiXmlBase::NUM_ENTITY ] = +{ + { "&", 5, '&' }, + { "<", 4, '<' }, + { ">", 4, '>' }, + { """, 6, '\"' }, + { "'", 6, '\'' } +}; + +// Bunch of unicode info at: +// http://www.unicode.org/faq/utf_bom.html +// Including the basic of this table, which determines the #bytes in the +// sequence from the lead byte. 1 placed for invalid sequences -- +// although the result will be junk, pass it through as much as possible. +// Beware of the non-characters in UTF-8: +// ef bb bf (Microsoft "lead bytes") +// ef bf be +// ef bf bf + +const unsigned char TIXML_UTF_LEAD_0 = 0xefU; +const unsigned char TIXML_UTF_LEAD_1 = 0xbbU; +const unsigned char TIXML_UTF_LEAD_2 = 0xbfU; + +const int TiXmlBase::utf8ByteTable[256] = +{ + // 0 1 2 3 4 5 6 7 8 9 a b c d e f + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0x00 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0x10 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0x20 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0x30 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0x40 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0x50 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0x60 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0x70 End of ASCII range + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0x80 0x80 to 0xc1 invalid + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0x90 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0xa0 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0xb0 + 1, 1, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, // 0xc0 0xc2 to 0xdf 2 byte + 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, // 0xd0 + 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, // 0xe0 0xe0 to 0xef 3 byte + 4, 4, 4, 4, 4, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 // 0xf0 0xf0 to 0xf4 4 byte, 0xf5 and higher invalid +}; + + +void TiXmlBase::ConvertUTF32ToUTF8( unsigned long input, char* output, int* length ) +{ + const unsigned long BYTE_MASK = 0xBF; + const unsigned long BYTE_MARK = 0x80; + const unsigned long FIRST_BYTE_MARK[7] = { 0x00, 0x00, 0xC0, 0xE0, 0xF0, 0xF8, 0xFC }; + + if (input < 0x80) + *length = 1; + else if ( input < 0x800 ) + *length = 2; + else if ( input < 0x10000 ) + *length = 3; + else if ( input < 0x200000 ) + *length = 4; + else + { *length = 0; return; } // This code won't covert this correctly anyway. + + output += *length; + + // Scary scary fall throughs. + switch (*length) + { + case 4: + --output; + *output = (char)((input | BYTE_MARK) & BYTE_MASK); + input >>= 6; + case 3: + --output; + *output = (char)((input | BYTE_MARK) & BYTE_MASK); + input >>= 6; + case 2: + --output; + *output = (char)((input | BYTE_MARK) & BYTE_MASK); + input >>= 6; + case 1: + --output; + *output = (char)(input | FIRST_BYTE_MARK[*length]); + } +} + + +/*static*/ int TiXmlBase::IsAlpha( unsigned char anyByte, TiXmlEncoding /*encoding*/ ) +{ + // This will only work for low-ascii, everything else is assumed to be a valid + // letter. I'm not sure this is the best approach, but it is quite tricky trying + // to figure out alhabetical vs. not across encoding. So take a very + // conservative approach. + +// if ( encoding == TIXML_ENCODING_UTF8 ) +// { + if ( anyByte < 127 ) + return isalpha( anyByte ); + else + return 1; // What else to do? The unicode set is huge...get the english ones right. +// } +// else +// { +// return isalpha( anyByte ); +// } +} + + +/*static*/ int TiXmlBase::IsAlphaNum( unsigned char anyByte, TiXmlEncoding /*encoding*/ ) +{ + // This will only work for low-ascii, everything else is assumed to be a valid + // letter. I'm not sure this is the best approach, but it is quite tricky trying + // to figure out alhabetical vs. not across encoding. So take a very + // conservative approach. + +// if ( encoding == TIXML_ENCODING_UTF8 ) +// { + if ( anyByte < 127 ) + return isalnum( anyByte ); + else + return 1; // What else to do? The unicode set is huge...get the english ones right. +// } +// else +// { +// return isalnum( anyByte ); +// } +} + + +class TiXmlParsingData +{ + friend class TiXmlDocument; + public: + void Stamp( const char* now, TiXmlEncoding encoding ); + + const TiXmlCursor& Cursor() const { return cursor; } + + private: + // Only used by the document! + TiXmlParsingData( const char* start, int _tabsize, int row, int col ) + { + assert( start ); + stamp = start; + tabsize = _tabsize; + cursor.row = row; + cursor.col = col; + } + + TiXmlCursor cursor; + const char* stamp; + int tabsize; +}; + + +void TiXmlParsingData::Stamp( const char* now, TiXmlEncoding encoding ) +{ + assert( now ); + + // Do nothing if the tabsize is 0. + if ( tabsize < 1 ) + { + return; + } + + // Get the current row, column. + int row = cursor.row; + int col = cursor.col; + const char* p = stamp; + assert( p ); + + while ( p < now ) + { + // Treat p as unsigned, so we have a happy compiler. + const unsigned char* pU = (const unsigned char*)p; + + // Code contributed by Fletcher Dunn: (modified by lee) + switch (*pU) { + case 0: + // We *should* never get here, but in case we do, don't + // advance past the terminating null character, ever + return; + + case '\r': + // bump down to the next line + ++row; + col = 0; + // Eat the character + ++p; + + // Check for \r\n sequence, and treat this as a single character + if (*p == '\n') { + ++p; + } + break; + + case '\n': + // bump down to the next line + ++row; + col = 0; + + // Eat the character + ++p; + + // Check for \n\r sequence, and treat this as a single + // character. (Yes, this bizarre thing does occur still + // on some arcane platforms...) + if (*p == '\r') { + ++p; + } + break; + + case '\t': + // Eat the character + ++p; + + // Skip to next tab stop + col = (col / tabsize + 1) * tabsize; + break; + + case TIXML_UTF_LEAD_0: + if ( encoding == TIXML_ENCODING_UTF8 ) + { + if ( *(p+1) && *(p+2) ) + { + // In these cases, don't advance the column. These are + // 0-width spaces. + if ( *(pU+1)==TIXML_UTF_LEAD_1 && *(pU+2)==TIXML_UTF_LEAD_2 ) + p += 3; + else if ( *(pU+1)==0xbfU && *(pU+2)==0xbeU ) + p += 3; + else if ( *(pU+1)==0xbfU && *(pU+2)==0xbfU ) + p += 3; + else + { p +=3; ++col; } // A normal character. + } + } + else + { + ++p; + ++col; + } + break; + + default: + if ( encoding == TIXML_ENCODING_UTF8 ) + { + // Eat the 1 to 4 byte utf8 character. + int step = TiXmlBase::utf8ByteTable[*((const unsigned char*)p)]; + if ( step == 0 ) + step = 1; // Error case from bad encoding, but handle gracefully. + p += step; + + // Just advance one column, of course. + ++col; + } + else + { + ++p; + ++col; + } + break; + } + } + cursor.row = row; + cursor.col = col; + assert( cursor.row >= -1 ); + assert( cursor.col >= -1 ); + stamp = p; + assert( stamp ); +} + + +const char* TiXmlBase::SkipWhiteSpace( const char* p, TiXmlEncoding encoding ) +{ + if ( !p || !*p ) + { + return 0; + } + if ( encoding == TIXML_ENCODING_UTF8 ) + { + while ( *p ) + { + const unsigned char* pU = (const unsigned char*)p; + + // Skip the stupid Microsoft UTF-8 Byte order marks + if ( *(pU+0)==TIXML_UTF_LEAD_0 + && *(pU+1)==TIXML_UTF_LEAD_1 + && *(pU+2)==TIXML_UTF_LEAD_2 ) + { + p += 3; + continue; + } + else if(*(pU+0)==TIXML_UTF_LEAD_0 + && *(pU+1)==0xbfU + && *(pU+2)==0xbeU ) + { + p += 3; + continue; + } + else if(*(pU+0)==TIXML_UTF_LEAD_0 + && *(pU+1)==0xbfU + && *(pU+2)==0xbfU ) + { + p += 3; + continue; + } + + if ( IsWhiteSpace( *p ) ) // Still using old rules for white space. + ++p; + else + break; + } + } + else + { + while ( *p && IsWhiteSpace( *p ) ) + ++p; + } + + return p; +} + +#ifdef TIXML_USE_STL +/*static*/ bool TiXmlBase::StreamWhiteSpace( std::istream * in, TIXML_STRING * tag ) +{ + for( ;; ) + { + if ( !in->good() ) return false; + + int c = in->peek(); + // At this scope, we can't get to a document. So fail silently. + if ( !IsWhiteSpace( c ) || c <= 0 ) + return true; + + *tag += (char) in->get(); + } +} + +/*static*/ bool TiXmlBase::StreamTo( std::istream * in, int character, TIXML_STRING * tag ) +{ + //assert( character > 0 && character < 128 ); // else it won't work in utf-8 + while ( in->good() ) + { + int c = in->peek(); + if ( c == character ) + return true; + if ( c <= 0 ) // Silent failure: can't get document at this scope + return false; + + in->get(); + *tag += (char) c; + } + return false; +} +#endif + +// One of TinyXML's more performance demanding functions. Try to keep the memory overhead down. The +// "assign" optimization removes over 10% of the execution time. +// +const char* TiXmlBase::ReadName( const char* p, TIXML_STRING * name, TiXmlEncoding encoding ) +{ + // Oddly, not supported on some comilers, + //name->clear(); + // So use this: + *name = ""; + assert( p ); + + // Names start with letters or underscores. + // Of course, in unicode, tinyxml has no idea what a letter *is*. The + // algorithm is generous. + // + // After that, they can be letters, underscores, numbers, + // hyphens, or colons. (Colons are valid ony for namespaces, + // but tinyxml can't tell namespaces from names.) + if ( p && *p + && ( IsAlpha( (unsigned char) *p, encoding ) || *p == '_' ) ) + { + const char* start = p; + while( p && *p + && ( IsAlphaNum( (unsigned char ) *p, encoding ) + || *p == '_' + || *p == '-' + || *p == '.' + || *p == ':' ) ) + { + //(*name) += *p; // expensive + ++p; + } + if ( p-start > 0 ) { + name->assign( start, p-start ); + } + return p; + } + return 0; +} + +const char* TiXmlBase::GetEntity( const char* p, char* value, int* length, TiXmlEncoding encoding ) +{ + // Presume an entity, and pull it out. + TIXML_STRING ent; + int i; + *length = 0; + + if ( *(p+1) && *(p+1) == '#' && *(p+2) ) + { + unsigned long ucs = 0; + ptrdiff_t delta = 0; + unsigned mult = 1; + + if ( *(p+2) == 'x' ) + { + // Hexadecimal. + if ( !*(p+3) ) return 0; + + const char* q = p+3; + q = strchr( q, ';' ); + + if ( !q || !*q ) return 0; + + delta = q-p; + --q; + + while ( *q != 'x' ) + { + if ( *q >= '0' && *q <= '9' ) + ucs += mult * (*q - '0'); + else if ( *q >= 'a' && *q <= 'f' ) + ucs += mult * (*q - 'a' + 10); + else if ( *q >= 'A' && *q <= 'F' ) + ucs += mult * (*q - 'A' + 10 ); + else + return 0; + mult *= 16; + --q; + } + } + else + { + // Decimal. + if ( !*(p+2) ) return 0; + + const char* q = p+2; + q = strchr( q, ';' ); + + if ( !q || !*q ) return 0; + + delta = q-p; + --q; + + while ( *q != '#' ) + { + if ( *q >= '0' && *q <= '9' ) + ucs += mult * (*q - '0'); + else + return 0; + mult *= 10; + --q; + } + } + if ( encoding == TIXML_ENCODING_UTF8 ) + { + // convert the UCS to UTF-8 + ConvertUTF32ToUTF8( ucs, value, length ); + } + else + { + *value = (char)ucs; + *length = 1; + } + return p + delta + 1; + } + + // Now try to match it. + for( i=0; iappend( cArr, len ); + } + } + else + { + bool whitespace = false; + + // Remove leading white space: + p = SkipWhiteSpace( p, encoding ); + while ( p && *p + && !StringEqual( p, endTag, caseInsensitive, encoding ) ) + { + if ( *p == '\r' || *p == '\n' ) + { + whitespace = true; + ++p; + } + else if ( IsWhiteSpace( *p ) ) + { + whitespace = true; + ++p; + } + else + { + // If we've found whitespace, add it before the + // new character. Any whitespace just becomes a space. + if ( whitespace ) + { + (*text) += ' '; + whitespace = false; + } + int len; + char cArr[4] = { 0, 0, 0, 0 }; + p = GetChar( p, cArr, &len, encoding ); + if ( len == 1 ) + (*text) += cArr[0]; // more efficient + else + text->append( cArr, len ); + } + } + } + if ( p && *p ) + p += strlen( endTag ); + return ( p && *p ) ? p : 0; +} + +#ifdef TIXML_USE_STL + +void TiXmlDocument::StreamIn( std::istream * in, TIXML_STRING * tag ) +{ + // The basic issue with a document is that we don't know what we're + // streaming. Read something presumed to be a tag (and hope), then + // identify it, and call the appropriate stream method on the tag. + // + // This "pre-streaming" will never read the closing ">" so the + // sub-tag can orient itself. + + if ( !StreamTo( in, '<', tag ) ) + { + SetError( TIXML_ERROR_PARSING_EMPTY, 0, 0, TIXML_ENCODING_UNKNOWN ); + return; + } + + while ( in->good() ) + { + int tagIndex = (int) tag->length(); + while ( in->good() && in->peek() != '>' ) + { + int c = in->get(); + if ( c <= 0 ) + { + SetError( TIXML_ERROR_EMBEDDED_NULL, 0, 0, TIXML_ENCODING_UNKNOWN ); + break; + } + (*tag) += (char) c; + } + + if ( in->good() ) + { + // We now have something we presume to be a node of + // some sort. Identify it, and call the node to + // continue streaming. + TiXmlNode* node = Identify( tag->c_str() + tagIndex, TIXML_DEFAULT_ENCODING ); + + if ( node ) + { + node->StreamIn( in, tag ); + bool isElement = node->ToElement() != 0; + delete node; + node = 0; + + // If this is the root element, we're done. Parsing will be + // done by the >> operator. + if ( isElement ) + { + return; + } + } + else + { + SetError( TIXML_ERROR, 0, 0, TIXML_ENCODING_UNKNOWN ); + return; + } + } + } + // We should have returned sooner. + SetError( TIXML_ERROR, 0, 0, TIXML_ENCODING_UNKNOWN ); +} + +#endif + +const char* TiXmlDocument::Parse( const char* p, TiXmlParsingData* prevData, TiXmlEncoding encoding ) +{ + ClearError(); + + // Parse away, at the document level. Since a document + // contains nothing but other tags, most of what happens + // here is skipping white space. + if ( !p || !*p ) + { + SetError( TIXML_ERROR_DOCUMENT_EMPTY, 0, 0, TIXML_ENCODING_UNKNOWN ); + return 0; + } + + // Note that, for a document, this needs to come + // before the while space skip, so that parsing + // starts from the pointer we are given. + location.Clear(); + if ( prevData ) + { + location.row = prevData->cursor.row; + location.col = prevData->cursor.col; + } + else + { + location.row = 0; + location.col = 0; + } + TiXmlParsingData data( p, TabSize(), location.row, location.col ); + location = data.Cursor(); + + if ( encoding == TIXML_ENCODING_UNKNOWN ) + { + // Check for the Microsoft UTF-8 lead bytes. + const unsigned char* pU = (const unsigned char*)p; + if ( *(pU+0) && *(pU+0) == TIXML_UTF_LEAD_0 + && *(pU+1) && *(pU+1) == TIXML_UTF_LEAD_1 + && *(pU+2) && *(pU+2) == TIXML_UTF_LEAD_2 ) + { + encoding = TIXML_ENCODING_UTF8; + useMicrosoftBOM = true; + } + } + + p = SkipWhiteSpace( p, encoding ); + if ( !p ) + { + SetError( TIXML_ERROR_DOCUMENT_EMPTY, 0, 0, TIXML_ENCODING_UNKNOWN ); + return 0; + } + + while ( p && *p ) + { + TiXmlNode* node = Identify( p, encoding ); + if ( node ) + { + p = node->Parse( p, &data, encoding ); + LinkEndChild( node ); + } + else + { + break; + } + + // Did we get encoding info? + if ( encoding == TIXML_ENCODING_UNKNOWN + && node->ToDeclaration() ) + { + TiXmlDeclaration* dec = node->ToDeclaration(); + const char* enc = dec->Encoding(); + assert( enc ); + + if ( *enc == 0 ) + encoding = TIXML_ENCODING_UTF8; + else if ( StringEqual( enc, "UTF-8", true, TIXML_ENCODING_UNKNOWN ) ) + encoding = TIXML_ENCODING_UTF8; + else if ( StringEqual( enc, "UTF8", true, TIXML_ENCODING_UNKNOWN ) ) + encoding = TIXML_ENCODING_UTF8; // incorrect, but be nice + else + encoding = TIXML_ENCODING_LEGACY; + } + + p = SkipWhiteSpace( p, encoding ); + } + + // Was this empty? + if ( !firstChild ) { + SetError( TIXML_ERROR_DOCUMENT_EMPTY, 0, 0, encoding ); + return 0; + } + + // All is well. + return p; +} + +void TiXmlDocument::SetError( int err, const char* pError, TiXmlParsingData* data, TiXmlEncoding encoding ) +{ + // The first error in a chain is more accurate - don't set again! + if ( error ) + return; + + assert( err > 0 && err < TIXML_ERROR_STRING_COUNT ); + error = true; + errorId = err; + errorDesc = errorString[ errorId ]; + + errorLocation.Clear(); + if ( pError && data ) + { + data->Stamp( pError, encoding ); + errorLocation = data->Cursor(); + } +} + + +TiXmlNode* TiXmlNode::Identify( const char* p, TiXmlEncoding encoding ) +{ + TiXmlNode* returnNode = 0; + + p = SkipWhiteSpace( p, encoding ); + if( !p || !*p || *p != '<' ) + { + return 0; + } + + p = SkipWhiteSpace( p, encoding ); + + if ( !p || !*p ) + { + return 0; + } + + // What is this thing? + // - Elements start with a letter or underscore, but xml is reserved. + // - Comments: "; + + if ( !StringEqual( p, startTag, false, encoding ) ) + { + if ( document ) + document->SetError( TIXML_ERROR_PARSING_COMMENT, p, data, encoding ); + return 0; + } + p += strlen( startTag ); + + // [ 1475201 ] TinyXML parses entities in comments + // Oops - ReadText doesn't work, because we don't want to parse the entities. + // p = ReadText( p, &value, false, endTag, false, encoding ); + // + // from the XML spec: + /* + [Definition: Comments may appear anywhere in a document outside other markup; in addition, + they may appear within the document type declaration at places allowed by the grammar. + They are not part of the document's character data; an XML processor MAY, but need not, + make it possible for an application to retrieve the text of comments. For compatibility, + the string "--" (double-hyphen) MUST NOT occur within comments.] Parameter entity + references MUST NOT be recognized within comments. + + An example of a comment: + + + */ + + value = ""; + // Keep all the white space. + while ( p && *p && !StringEqual( p, endTag, false, encoding ) ) + { + value.append( p, 1 ); + ++p; + } + if ( p && *p ) + p += strlen( endTag ); + + return p; +} + + +const char* TiXmlAttribute::Parse( const char* p, TiXmlParsingData* data, TiXmlEncoding encoding ) +{ + p = SkipWhiteSpace( p, encoding ); + if ( !p || !*p ) return 0; + + if ( data ) + { + data->Stamp( p, encoding ); + location = data->Cursor(); + } + // Read the name, the '=' and the value. + const char* pErr = p; + p = ReadName( p, &name, encoding ); + if ( !p || !*p ) + { + if ( document ) document->SetError( TIXML_ERROR_READING_ATTRIBUTES, pErr, data, encoding ); + return 0; + } + p = SkipWhiteSpace( p, encoding ); + if ( !p || !*p || *p != '=' ) + { + if ( document ) document->SetError( TIXML_ERROR_READING_ATTRIBUTES, p, data, encoding ); + return 0; + } + + ++p; // skip '=' + p = SkipWhiteSpace( p, encoding ); + if ( !p || !*p ) + { + if ( document ) document->SetError( TIXML_ERROR_READING_ATTRIBUTES, p, data, encoding ); + return 0; + } + + const char* end; + const char SINGLE_QUOTE = '\''; + const char DOUBLE_QUOTE = '\"'; + + if ( *p == SINGLE_QUOTE ) + { + ++p; + end = "\'"; // single quote in string + p = ReadText( p, &value, false, end, false, encoding ); + } + else if ( *p == DOUBLE_QUOTE ) + { + ++p; + end = "\""; // double quote in string + p = ReadText( p, &value, false, end, false, encoding ); + } + else + { + // All attribute values should be in single or double quotes. + // But this is such a common error that the parser will try + // its best, even without them. + value = ""; + while ( p && *p // existence + && !IsWhiteSpace( *p ) // whitespace + && *p != '/' && *p != '>' ) // tag end + { + if ( *p == SINGLE_QUOTE || *p == DOUBLE_QUOTE ) { + // [ 1451649 ] Attribute values with trailing quotes not handled correctly + // We did not have an opening quote but seem to have a + // closing one. Give up and throw an error. + if ( document ) document->SetError( TIXML_ERROR_READING_ATTRIBUTES, p, data, encoding ); + return 0; + } + value += *p; + ++p; + } + } + return p; +} + +#ifdef TIXML_USE_STL +void TiXmlText::StreamIn( std::istream * in, TIXML_STRING * tag ) +{ + while ( in->good() ) + { + int c = in->peek(); + if ( !cdata && (c == '<' ) ) + { + return; + } + if ( c <= 0 ) + { + TiXmlDocument* document = GetDocument(); + if ( document ) + document->SetError( TIXML_ERROR_EMBEDDED_NULL, 0, 0, TIXML_ENCODING_UNKNOWN ); + return; + } + + (*tag) += (char) c; + in->get(); // "commits" the peek made above + + if ( cdata && c == '>' && tag->size() >= 3 ) { + size_t len = tag->size(); + if ( (*tag)[len-2] == ']' && (*tag)[len-3] == ']' ) { + // terminator of cdata. + return; + } + } + } +} +#endif + +const char* TiXmlText::Parse( const char* p, TiXmlParsingData* data, TiXmlEncoding encoding ) +{ + value = ""; + TiXmlDocument* document = GetDocument(); + + if ( data ) + { + data->Stamp( p, encoding ); + location = data->Cursor(); + } + + const char* const startTag = ""; + + if ( cdata || StringEqual( p, startTag, false, encoding ) ) + { + cdata = true; + + if ( !StringEqual( p, startTag, false, encoding ) ) + { + if ( document ) + document->SetError( TIXML_ERROR_PARSING_CDATA, p, data, encoding ); + return 0; + } + p += strlen( startTag ); + + // Keep all the white space, ignore the encoding, etc. + while ( p && *p + && !StringEqual( p, endTag, false, encoding ) + ) + { + value += *p; + ++p; + } + + TIXML_STRING dummy; + p = ReadText( p, &dummy, false, endTag, false, encoding ); + return p; + } + else + { + bool ignoreWhite = true; + + const char* end = "<"; + p = ReadText( p, &value, ignoreWhite, end, false, encoding ); + if ( p && *p ) + return p-1; // don't truncate the '<' + return 0; + } +} + +#ifdef TIXML_USE_STL +void TiXmlDeclaration::StreamIn( std::istream * in, TIXML_STRING * tag ) +{ + while ( in->good() ) + { + int c = in->get(); + if ( c <= 0 ) + { + TiXmlDocument* document = GetDocument(); + if ( document ) + document->SetError( TIXML_ERROR_EMBEDDED_NULL, 0, 0, TIXML_ENCODING_UNKNOWN ); + return; + } + (*tag) += (char) c; + + if ( c == '>' ) + { + // All is well. + return; + } + } +} +#endif + +const char* TiXmlDeclaration::Parse( const char* p, TiXmlParsingData* data, TiXmlEncoding _encoding ) +{ + p = SkipWhiteSpace( p, _encoding ); + // Find the beginning, find the end, and look for + // the stuff in-between. + TiXmlDocument* document = GetDocument(); + if ( !p || !*p || !StringEqual( p, "SetError( TIXML_ERROR_PARSING_DECLARATION, 0, 0, _encoding ); + return 0; + } + if ( data ) + { + data->Stamp( p, _encoding ); + location = data->Cursor(); + } + p += 5; + + version = ""; + encoding = ""; + standalone = ""; + + while ( p && *p ) + { + if ( *p == '>' ) + { + ++p; + return p; + } + + p = SkipWhiteSpace( p, _encoding ); + if ( StringEqual( p, "version", true, _encoding ) ) + { + TiXmlAttribute attrib; + p = attrib.Parse( p, data, _encoding ); + version = attrib.Value(); + } + else if ( StringEqual( p, "encoding", true, _encoding ) ) + { + TiXmlAttribute attrib; + p = attrib.Parse( p, data, _encoding ); + encoding = attrib.Value(); + } + else if ( StringEqual( p, "standalone", true, _encoding ) ) + { + TiXmlAttribute attrib; + p = attrib.Parse( p, data, _encoding ); + standalone = attrib.Value(); + } + else + { + // Read over whatever it is. + while( p && *p && *p != '>' && !IsWhiteSpace( *p ) ) + ++p; + } + } + return 0; +} + +bool TiXmlText::Blank() const +{ + for ( unsigned i=0; i